From ffa4d24f18598134876f2f02b96455677f63bad0 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 11 Aug 2026 17:07:11 +1000 Subject: [PATCH 01/20] G12.1, G13.1: select kinematics from G-code G12.1 P- selects one of the kinematics offered by a switchable kinematics module and G13.1 cancels back to kinematics 0. Both are queue synchronisation points, so no motion is ever planned in one kinematics and executed in another. Until now the only way to switch from a program was to write motion.switchkins-type through an analog output and force a sync by hand, typically M68 E3 Q1 followed by M66 E0 L0, wrapped in a subroutine or a remapped M-code. That also costs the #5399 variable on every switch, because M66 writes it. G13.1 cancels to kinematics 0 rather than restoring whatever was selected before, which is how every other cancel in the language behaves and keeps a block's meaning independent of the path taken through the program. To put back a caller's selection, read #<_kins_type>: # = #<_kins_type> G12.1 P2 ( ... ) G12.1 P# Nothing cancels the selection implicitly. It survives program end and abort so that the kinematics keeps matching the position readout, since switching re-derives world position from the joints and would otherwise move the readout while the machine stands still. Motion takes the G-code request and the motion.switchkins-type pin on their edges, so whichever asked most recently wins and a config can use either or both. Writing the pin from motion instead does not work: the configs source it from an analog output that would put its own value back on the next servo cycle. motion.kins-type reports the selection now in force. Q was parsed and carried all the way to motion without anything ever reading it, so it is gone. EMC_ADJUST_KINS_OFFSET_DATA is registered in the NML format and name tables and has the update() its declaration promised, without which the message could not cross the channel. --- docs/src/gcode/g-code.adoc | 61 ++++++++++++++++++++ docs/src/gcode/overview.adoc | 4 ++ docs/src/man/man9/motion.9.adoc | 6 ++ docs/src/motion/switchkins.adoc | 77 ++++++++++++++++++++------ src/emc/motion/command.c | 10 ++++ src/emc/motion/control.c | 26 ++++++++- src/emc/motion/mot_priv.h | 1 + src/emc/motion/motion.c | 1 + src/emc/motion/motion.h | 10 ++++ src/emc/nml_intf/canon.hh | 3 + src/emc/nml_intf/emc.cc | 12 ++++ src/emc/nml_intf/emc.hh | 5 +- src/emc/nml_intf/emc_nml.hh | 20 ++++++- src/emc/nml_intf/emcops.cc | 5 +- src/emc/rs274ngc/gcodemodule.cc | 7 +++ src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_check.cc | 9 ++- src/emc/rs274ngc/interp_convert.cc | 47 +++++++++++++++- src/emc/rs274ngc/interp_execute.cc | 3 + src/emc/rs274ngc/interp_internal.hh | 4 ++ src/emc/rs274ngc/interp_namedparams.cc | 8 +++ src/emc/rs274ngc/interp_setup.cc | 2 + src/emc/rs274ngc/rs274ngc_interp.hh | 1 + src/emc/rs274ngc/rs274ngc_pre.cc | 9 +++ src/emc/rs274ngc/rs274ngc_return.hh | 3 + src/emc/sai/saicanon.cc | 8 +++ src/emc/task/emccanon.cc | 11 ++++ src/emc/task/emctaskmain.cc | 27 +++++++++ src/emc/task/taskintf.cc | 13 +++++ tests/remap/introspect/expected | 4 +- 30 files changed, 372 insertions(+), 27 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 6253e57afb2..e173c9b54c7 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -70,6 +70,7 @@ as the 'L number', and so on for any other letter. |<> |Set Tool Table, Calculated, Fixture |<> |Coordinate System Origin Setting |<> |Coordinate System Origin Setting Calculated +|<> |Select Kinematics |<> |Plane Select |<> |Set Units of Measure |<> |Go to Predefined Position @@ -934,6 +935,66 @@ It is an error if: * The P number does not evaluate to an integer in the range 0 to 9. * An axis is programmed that is not defined in the configuration. +[[gcode:g12.1-g13.1]] +== G12.1, G13.1 Select Kinematics(((G12.1, G13.1 Select Kinematics))) + +---- +G12.1 P- +G13.1 +---- + +'G12.1' selects one of the kinematics provided by a switchable kinematics +module, and 'G13.1' cancels back to kinematics 0. The 'P' word is the +kinematics number, the same number that the `motion.switchkins-type` pin +takes, so 'G13.1' and `G12.1 P0` do the same thing. A config may select +the kinematics from G-code, from that pin, or from both: each is acted on +when it changes, so the most recent request is the one in force. + +Both codes are queue synchronisation points. The interpreter waits for +queued motion to finish before the kinematics changes, so no move is ever +planned in one kinematics and executed in another. Because of that, both +codes stop any blending that was in progress, in the same way 'G4' does. + +The kinematics module decides what each number means. See the +`switchkins` section of the kins(9) man page for the modules that support +switching and the order in which they list their kinematics. A machine +whose kinematics module is not switchable rejects the change. + +Selecting a kinematics does not move the machine. It changes how joint +positions and coordinate positions map onto each other, so the position +readout can change even though nothing has moved. + +The active kinematics is available to the program as the read-only +parameter '#<_kins_type>', which lets a subroutine put back whatever was +selected before it ran: + +[source,ngc] +---- +# = #<_kins_type> +G12.1 P2 (work in kinematics 2) +( ... ) +G12.1 P# (put back whatever the caller was using) +---- + +Nothing cancels the selection on its own. It survives the end of the +program and an abort, so that the kinematics keeps matching what the +position readout shows. End a program with 'G13.1' if it should leave the +machine in kinematics 0. + +.G12.1, G13.1 Example +[source,ngc] +---- +G12.1 P1 (switch to kinematics 1) +G0 X0 Y0 +G13.1 (back to kinematics 0) +---- + +It is an error if: + +* 'G12.1' is used without a 'P' word. +* The 'P' word is negative. +* A 'P' word is used with 'G13.1'. + [[gcode:g17-g19.1]] == G17 - G19.1 Plane Select(((G17 - G19.1 Plane Select))) diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 683f72cc154..4ed0ec7c73d 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -503,6 +503,10 @@ can be added easily without changes to the source code. | G89 | 890 |=== +* '#<_kins_type>' - Kinematics selected by 'G12.1' or 'G13.1'. Returns the + 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics + has been selected. See <>. + * '#<_plane>' - returns the value designating the current plane: [width="20%",options="header"] diff --git a/docs/src/man/man9/motion.9.adoc b/docs/src/man/man9/motion.9.adoc index 43859fcef14..8b1c78a936e 100644 --- a/docs/src/man/man9/motion.9.adoc +++ b/docs/src/man/man9/motion.9.adoc @@ -256,6 +256,12 @@ Note: feed-inhibit applies to G-code commands -- not jogs. select the machine kinematics functions. Extra G-code commands may be required to synchronize task and motion before and after changes to the pin value. + The G-code words *G12.1 P-* and *G13.1* write this pin and synchronize + task and motion themselves, so a program that uses them needs no such + extra commands. +*motion.kins-type* OUT float:: + The kinematics currently selected, echoing the value that was last + applied from *motion.switchkins-type*. *motion.teleop-mode* OUT BIT:: Motion mode is teleop (axis coordinate jogging available). *motion.tooloffset.L* OUT FLOAT:: diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index a250825cd3e..d02633d1515 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -20,17 +20,18 @@ specific kinematics calculations for most operations but can be switched to identity kinematics for control of individual joints after homing. -The kinematics type is selected by a motion module HAL pin that -can be updated from a G-code program or by interactive MDI -commands. The halui provisions for activating MDI commands can be -used to allow buttons to select the kinematics type using -hardware controls or a virtual panel (PyVCP, GladeVCP, etc.). - -When a kinematics type is changed, the G-code must also issue -commands to *force synchronization* of the interpreter and motion -parts of LinuxCNC. Typically, a HAL pin 'read' command (M66 E0 L0) is -used immediately after altering the controlling HAL pin to force -synchronization. +The kinematics type is selected with 'G12.1 P-' and 'G13.1', from a +G-code program or by interactive MDI commands. It can also be selected +by a motion module HAL pin, which allows the halui provisions for +activating MDI commands to be used so that buttons select the +kinematics type from hardware controls or a virtual panel (PyVCP, +GladeVCP, etc.). + +Changing the kinematics type requires the interpreter and motion parts +of LinuxCNC to be *synchronized*. 'G12.1' and 'G13.1' do this +themselves. When the HAL pin is written instead, the G-code must force +synchronization, typically with a HAL pin 'read' command (M66 E0 L0) +immediately after altering the pin. == Switchable Kinematic Modules @@ -124,6 +125,7 @@ program behavior in accordance with the active kinematics type. === HAL Pin Summary . *motion.switchkins-type* Input (float) +. *motion.kins-type* Output (float) . *kinstype.is-0* Output (bit) . *kinstype.is-1* Output (bit) . *kinstype.is-2* Output (bit) @@ -136,9 +138,10 @@ A module providing more than three kinematics types has one === HAL Connections Switchkins functionality is enabled by the pin -*motion.switchkins-type*. Typically, this pin is sourced by an -analog output pin like motion.analog-out-03 so that it can be -set by M68 commands. Example: +*motion.switchkins-type*, which 'G12.1' and 'G13.1' write directly. +To select a kinstype from HAL instead, source the pin from an analog +output pin like motion.analog-out-03 so that it can be set by M68 +commands. Example: [source,hal] ---- @@ -146,9 +149,51 @@ net :kinstype-select <= motion.analog-out-03 net :kinstype-select => motion.switchkins-type ---- -=== G-/M-code commands +=== G-code commands -Kinstype selection is managed using G-code sequences like: +'G12.1 P-' selects a kinstype and 'G13.1' cancels back to kinstype 0: + +[source,ngc] +---- +... +G12.1 P1 ;select kinstype 1 +... +... ;user G-code +... +G13.1 ;back to kinstype 0 +... +---- + +These codes ask motion for the kinstype directly and synchronize task and +motion themselves, so no HAL connection and no separate sync command are +needed. The G-code words and the *motion.switchkins-type* pin are both +acted on when they change, so whichever asked most recently is the one in +force, and a config can use either or both. *motion.kins-type* reports +what is currently selected. + +The kinstype in force is readable in G-code as '#<_kins_type>', which lets +a subroutine restore whatever its caller had selected: + +[source,ngc] +---- +# = #<_kins_type> +G12.1 P2 +( ... ) +G12.1 P# +---- + +Selection is not cancelled by the end of a program or by an abort, so +that the kinstype continues to match the position readout. A program +that should leave the machine in kinstype 0 ends with 'G13.1'. + +See the G-code documentation for 'G12.1' and 'G13.1' for the full +description. + +=== M-code commands + +A kinstype can also be selected by writing *motion.switchkins-type* +through an analog output pin, which needs the HAL connection shown +above. Kinstype selection is then managed using G-code sequences like: [source,ngc] ---- diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 8905ad05d13..1a48585fb5e 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -2054,6 +2054,16 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) axis_set_locking_joint(emcmotCommand->axis, joint_num); break; + case EMCMOT_ADJUST_KINS_OFFSET_DATA: + emcmotConfig->adjustKinsVar0 = emcmotCommand->adjustKinsVar0; + if(emcmotConfig->kinsType == 'r'){ + emcmotConfig->kinsType = 's'; + } + else{ + emcmotConfig->kinsType = 'r'; + } + break; + default: rtapi_print_msg(RTAPI_MSG_DBG, "UNKNOWN"); reportError(_("unrecognized command %d"), emcmotCommand->command); diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 2ddf587b484..92f854431a0 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -300,12 +300,34 @@ static bool joint_jog_is_active(void) { static void handle_kinematicsSwitch(void) { int joint_num; int hal_switchkins_type = 0; + static int prev_hal_switchkins_type = 0; + int requested_type; if (!kinematicsSwitchable()) return; + + /* Two things can ask for a kinematics: G12.1/G13.1, and the + motion.switchkins-type pin. Both are taken on their edge, so that + whichever asked most recently wins. Writing the pin here instead + would not work: configs source it from an analog output, which + would put its own value back on the next servo cycle. */ hal_switchkins_type = (int)hal_get_real(emcmot_hal_data->switchkins_type); - if (switchkins_type == hal_switchkins_type) return; + requested_type = switchkins_type; + + if (emcmotStatus->kinsType != emcmotConfig->kinsType) { + requested_type = (int)emcmotConfig->adjustKinsVar0; + emcmotStatus->kinsType = emcmotConfig->kinsType; + } else if (hal_switchkins_type != prev_hal_switchkins_type) { + requested_type = hal_switchkins_type; + } + prev_hal_switchkins_type = hal_switchkins_type; + + hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); + emcmotStatus->adjustKinsVar0 = switchkins_type; + if (switchkins_type == requested_type) return; - switchkins_type = hal_switchkins_type; + switchkins_type = requested_type; + hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); + emcmotStatus->adjustKinsVar0 = switchkins_type; emcmot_joint_t *jointKinsSwitch; double joint_posKinsSwitch[EMCMOT_MAX_JOINTS] = {0,}; diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index 64cbb507132..a996e183fa6 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -198,6 +198,7 @@ typedef struct { hal_real_t feed_mm_per_second; /* feed mm per second*/ hal_real_t switchkins_type; + hal_real_t kins_type; /* Interp State Pins */ hal_sint_t interp_line_number; hal_sint_t interp_motion_type; diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index d2cb7615958..9a7985902fe 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -661,6 +661,7 @@ static int init_hal_io(void) if (kinematicsSwitchable()) { CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_IN, &(emcmot_hal_data->switchkins_type), 0.0, "motion.switchkins-type")); + CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &(emcmot_hal_data->kins_type), 0.0, "motion.kins-type")); } /* export spindle pins and params */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 1312b5e45dd..9a7bf7959a7 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -176,6 +176,8 @@ extern "C" { EMCMOT_SET_AXIS_LOCKING_JOINT, /* set the axis locking joint */ EMCMOT_SET_AXIS_JERK_LIMIT, /* set the max axis jerk */ + EMCMOT_ADJUST_KINS_OFFSET_DATA, /* set the offset in kins (G12.1) */ + EMCMOT_SET_SPINDLE_PARAMS, /* One command to set all spindle params */ } cmd_code_t; @@ -270,6 +272,8 @@ extern "C" { 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; + + double adjustKinsVar0; } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars @@ -667,6 +671,9 @@ Suggestion: Split this in to an Error and a Status flag register.. int numExtraJoints; int stepping; bool jogging_active; + + char kinsType; + double adjustKinsVar0; } emcmot_status_t; /********************************* @@ -738,6 +745,9 @@ Suggestion: Split this in to an Error and a Status flag register.. double maxFeedScale; int inhibit_probe_jog_error; int inhibit_probe_home_error; + + double adjustKinsVar0; + char kinsType; } emcmot_config_t; /* error structure - lockfree MPSC ring buffer. See emcmotutil.c. */ diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 916b3e92971..e49242e3c77 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -1070,4 +1070,7 @@ extern int GET_EXTERNAL_OFFSET_APPLIED(); extern EmcPose GET_EXTERNAL_OFFSETS(); extern void UPDATE_TAG(const StateTag& tag); +// adjust kins offset (G12.1 kinematics switch) +extern void ADJUST_KINS_OFFSET(double adjustKinsVar0); + #endif /* ifndef CANON_HH */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 3eb7360bdd3..848db4990da 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -296,6 +296,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_OFFSET_TYPE: ((EMC_TRAJ_SET_OFFSET *) buffer)->update(cms); break; + case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: + ((EMC_ADJUST_KINS_OFFSET_DATA *) buffer)->update(cms); + break; case EMC_TRAJ_SET_G5X_TYPE: ((EMC_TRAJ_SET_G5X *) buffer)->update(cms); break; @@ -520,6 +523,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_MODE"; case EMC_TRAJ_SET_OFFSET_TYPE: return "EMC_TRAJ_SET_OFFSET"; + case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: + return "EMC_ADJUST_KINS_OFFSET_DATA"; case EMC_TRAJ_SET_G5X_TYPE: return "EMC_TRAJ_SET_G5X"; case EMC_TRAJ_SET_G92_TYPE: @@ -1591,6 +1596,13 @@ void EMC_TRAJ_SET_OFFSET::update(CMS * cms) EmcPose_update(cms, &offset); } +// cppcheck-suppress duplInheritedMember +void EMC_ADJUST_KINS_OFFSET_DATA::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + cms->update(adjustKinsVar0); +} + /* * NML/CMS Update function for EMC_TRAJ_CMD_MSG * Automatically generated by NML CodeGen Java Applet. diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 2738b34144b..91688da73c1 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -112,6 +112,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) +#define EMC_ADJUST_KINS_OFFSET_DATA_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) // EMC_MOTION aggregate class type declaration @@ -214,7 +215,8 @@ enum class EMC_TASK_EXEC { WAITING_FOR_MOTION_AND_IO = 7, WAITING_FOR_DELAY = 8, WAITING_FOR_SYSTEM_CMD = 9, - WAITING_FOR_SPINDLE_ORIENTED = 10 + WAITING_FOR_SPINDLE_ORIENTED = 10, + WAITING_FOR_KINS_SWITCH = 11 }; // types for EMC_TASK interpState @@ -460,6 +462,7 @@ int emcSetupArcBlends(int arcBlendEnable, int emcSetProbeErrorInhibit(int j_inhibit, int h_inhibit); int emcGetExternalOffsetApplied(void); EmcPose emcGetExternalOffsets(void); +extern int emcAdjustKinsOffset(double adjustKinsVar0); extern int emcUpdate(EMC_STAT * stat); // full EMC status diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 5cede52b09f..bb88a94ea75 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -960,13 +960,27 @@ class EMC_TRAJ_RIGID_TAP:public EMC_TRAJ_CMD_MSG { double vel, ini_maxvel, acc, scale, ini_maxjerk; }; +class EMC_ADJUST_KINS_OFFSET_DATA:public EMC_TRAJ_CMD_MSG { + public: + EMC_ADJUST_KINS_OFFSET_DATA():EMC_TRAJ_CMD_MSG(EMC_ADJUST_KINS_OFFSET_DATA_TYPE, + sizeof(EMC_ADJUST_KINS_OFFSET_DATA)), + adjustKinsVar0(0.0) + {}; + + double adjustKinsVar0; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); +}; + // EMC_TRAJ status base class class EMC_TRAJ_STAT_MSG:public RCS_STAT_MSG { public: EMC_TRAJ_STAT_MSG(NMLTYPE t, size_t s) : RCS_STAT_MSG(t, s) {}; - // For internal NML/CMS use only. void update(CMS * cms); }; @@ -1167,6 +1181,10 @@ class EMC_MOTION_STAT:public EMC_MOTION_STAT_MSG { int numExtraJoints; bool jogging_active; uint64_t heartbeat; // motion controller's heartbeat counter + + char trajKinsType; + bool trajKinsTypeModified; + double adjustKinsVar0; }; // declarations for EMC_TASK classes diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index 916e3e3e49a..49868ce1047 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -111,7 +111,10 @@ EMC_MOTION_STAT::EMC_MOTION_STAT() eoffset_pose{}, numExtraJoints(0), jogging_active(0), - heartbeat(0) + heartbeat(0), + trajKinsType(0), + trajKinsTypeModified(false), + adjustKinsVar0(0.0) { } diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 3b15edea612..f28092f6e41 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -890,6 +890,13 @@ void ON_RESET() {} void PALLET_SHUTTLE() {} void SELECT_TOOL(int tool) {selected_tool = tool;} void UPDATE_TAG(const StateTag& /*tag*/) {} +void ADJUST_KINS_OFFSET(double adjustKinsVar0) +{ + (void)adjustKinsVar0; + printf("gcodemodule: ADJUST_KINS_OFFSET\n"); + + return; +} void OPTIONAL_PROGRAM_STOP() {} int GET_EXTERNAL_TC_FAULT() {return 0;} int GET_EXTERNAL_TC_REASON() {return 0;} diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 74897b34650..63f6d20975a 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -76,7 +76,7 @@ const int Interp::gees[] = { /* 60 */ 1, 1, 1, 0,-1,-1,-1,-1,-1,-1,15,-1,-1,-1,-1,-1,-1,-1,-1,-1, // jjf added G6 /* 80 */ 15,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 100 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 120 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 120 */ -1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1, /* 140 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 160 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 2, 2,-1,-1,-1,-1,-1,-1,-1,-1, /* 180 */ 2, 2,-1,-1,-1,-1,-1,-1,-1,-1, 2, 2,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 196f2772763..40a1fe152fd 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -109,6 +109,11 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), NCE_CANNOT_USE_G53_INCREMENTAL); } else if (mode0 == G_92) { + } else if (mode0 == G_12_1){ + // kins-switch + CHKS((!block->p_flag), NCE_P_WORD_MISSING_WITH_G121); + } else if (mode0 == G_13_1){ + // kins-switch cancel: no words, the kinematics goes back to 0 } else ERS(NCE_BUG_BAD_G_CODE_MODAL_GROUP_0); return INTERP_OK; @@ -319,7 +324,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } if (block->p_flag) { - CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64) && + CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && (motion != G_76) && (motion != G_82) && (motion != G_86) && (motion != G_88) && (motion != G_89) && (motion != G_5) && (motion != G_5_2) && (motion != G_70) && @@ -331,7 +336,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G64 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G64 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); int p_value = round_to_int(block->p_number); diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 638e87eb0c3..1c3394812bc 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4351,7 +4351,20 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_axis_offsets(code, block, settings)); } else if ((code == G_5_3)||(code == G_6_3)) { // jjf CHP(convert_nurbs(code, block, settings)); - } else if ((code == G_4) || (code == G_53)); // handled elsewhere + } else if ((code == G_4) || (code == G_53)); // handled elsewhere + else if ((code == G_12_1) || (code == G_13_1)) { + // The flag makes the interpreter wait for motion to drain, so that no + // motion is ever planned across a change of kinematics. With nothing + // queued there is nothing to wait for, and asking to wait is actively + // harmful: an ON_ABORT_COMMAND routine is run by a single execute() + // call that cannot service INTERP_EXECUTE_FINISH, so the rest of the + // routine would be silently dropped. The queue is empty there because + // the abort has just flushed it. + if (!GET_EXTERNAL_QUEUE_EMPTY()) { + settings->kinsSwitch_flag = true; + } + CHP(convert_kins_switch(code, block, settings)); + } else ERS(NCE_BUG_CODE_NOT_G4_G10_G28_G30_G52_G53_OR_G92_SERIES); return INTERP_OK; @@ -6490,6 +6503,38 @@ int Interp::convert_tool_select(block_pointer block, //!< pointer to a block return INTERP_OK; } +/*! convert_kins_switch + +Returned Value: int (INTERP_OK) + +Side effects: + The selected kinematics is sent to the motion controller and recorded + in the interpreter so that #<_kins_type> reports it. + +Called by: convert_modal_0 + +G12.1 P- selects a kinematics; G13.1 cancels back to kinematics 0, which +is the same thing as G12.1 P0 and exists so that the pair reads the way +it does on other controls. Both are queue synchronisation points: the +caller sets kinsSwitch_flag, which makes the interpreter wait for motion +to drain before the switch takes effect, so no motion is ever planned +across a change of kinematics. + +*/ + +int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 + block_pointer block, //!< pointer to a block of RS274 instructions + setup_pointer settings) //!< pointer to machine settings +{ + int kins_type = (code == G_13_1) ? 0 : round_to_int(block->p_number); + + CHKS((kins_type < 0), _("G12.1 requires a non-negative P word")); + + ADJUST_KINS_OFFSET((double)kins_type); + settings->kins_type = kins_type; + return INTERP_OK; +} + int Interp::update_tag(StateTag &tag) { diff --git a/src/emc/rs274ngc/interp_execute.cc b/src/emc/rs274ngc/interp_execute.cc index e30635d8810..5863fc168e9 100644 --- a/src/emc/rs274ngc/interp_execute.cc +++ b/src/emc/rs274ngc/interp_execute.cc @@ -325,6 +325,9 @@ int Interp::execute_block(block_pointer block, //!< pointer to a block of RS27 if (settings->toolchange_flag) return (INTERP_EXECUTE_FINISH); + if (settings->kinsSwitch_flag) + return (INTERP_EXECUTE_FINISH); + // All changes to settings are complete write_canon_state_tag(block, settings); return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 22268854211..0069c1f178f 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -211,6 +211,8 @@ enum GCodes G_7 = 70, G_8 = 80, G_10 = 100, + G_12_1 = 121, + G_13_1 = 131, G_17 = 170, G_17_1 = 171, G_18 = 180, @@ -747,6 +749,8 @@ struct setup CANON_PLANE plane; // active plane, XY-, YZ-, or XZ-plane bool probe_flag; // flag indicating probing done bool input_flag; // flag indicating waiting for input done + bool kinsSwitch_flag; // flag indicating waiting for kinematics switch done + int kins_type; // kinematics selected by G12.1/G13.1 bool toolchange_flag; // flag indicating we just had a tool change int input_index; // channel queried bool input_digital; // input queried was digital (false=analog) diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index d0f2d4b8b63..fb12f1e8d26 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -58,6 +58,7 @@ using namespace linuxcnc; enum predefined_named_parameters { NP_LINE, NP_MOTION_MODE, + NP_KINS_TYPE, NP_PLANE, NP_CCOMP, NP_METRIC, @@ -541,6 +542,10 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.motion_mode; break; + case NP_KINS_TYPE: // _kins_type + *value = _setup.kins_type; + break; + case NP_PLANE: // _plane switch(_setup.plane) { case CANON_PLANE::XY: @@ -890,6 +895,9 @@ int Interp::init_named_parameters() init_readonly_param("_motion_mode", NP_MOTION_MODE, PA_USE_LOOKUP); + // kinematics selected by G12.1 P- / G13.1, 0 when none has been selected + init_readonly_param("_kins_type", NP_KINS_TYPE, PA_USE_LOOKUP); + // G17/18/19/17.1/18.1/19.1 -> return 170/180/190/171/181/191 init_readonly_param("_plane", NP_PLANE, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 365e4682d6c..29161513258 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -116,6 +116,8 @@ setup::setup() : plane(CANON_PLANE::XY), probe_flag(0), input_flag(0), + kinsSwitch_flag(0), + kins_type(0), toolchange_flag(0), input_index(0), input_digital(0), diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index af7d27e58bc..9d2cbadeb36 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -357,6 +357,7 @@ public: int convert_tool_length_offset(int g_code, block_pointer block, setup_pointer settings); int convert_tool_select(block_pointer block, setup_pointer settings); + int convert_kins_switch(int code, block_pointer block, setup_pointer settings); int update_tag(StateTag &tag); int cycle_feed(block_pointer block, CANON_PLANE plane, double end1, double end2, double end3); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 54212aa1850..16208923637 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1196,6 +1196,7 @@ int Interp::init() _setup.probe_flag = false; _setup.toolchange_flag = false; _setup.input_flag = false; + _setup.kinsSwitch_flag = false; _setup.input_index = -1; _setup.input_digital = false; _setup.program_x = 0.; /* for cutter comp */ @@ -1477,6 +1478,13 @@ int Interp::read_inputs(setup_pointer settings) } settings->input_flag = false; } + + if( settings->kinsSwitch_flag ){ + CHKS((GET_EXTERNAL_QUEUE_EMPTY() == 0), NCE_QUEUE_IS_NOT_EMPTY_AFTER_KINS_SWITCH); + + settings->kinsSwitch_flag = false; + } + return INTERP_OK; } @@ -2677,6 +2685,7 @@ int Interp::on_abort(int reason, const char *message) _setup.toolchange_flag = false; _setup.probe_flag = false; _setup.input_flag = false; + _setup.kinsSwitch_flag = false; if (_setup.on_abort_command == NULL) { return -1; diff --git a/src/emc/rs274ngc/rs274ngc_return.hh b/src/emc/rs274ngc/rs274ngc_return.hh index 9f6d8674b84..3cd733da7f3 100644 --- a/src/emc/rs274ngc/rs274ngc_return.hh +++ b/src/emc/rs274ngc/rs274ngc_return.hh @@ -196,6 +196,8 @@ #define NCE_CANNOT_CHANGE_PLANES_WITH_CUTTER_RADIUS_COMP_ON _("Cannot change planes with cutter radius compensation on") #define NCE_RADIUS_COMP_ONLY_IN_XY_OR_XZ _("Cutter radius compensation allowed only in XY, XZ planes") #define NCE_P_WORD_MISSING_WITH_G76 _("P word missing with G76") +#define NCE_P_WORD_MISSING_WITH_G121 _("P word missing with G12.1") +#define NCE_Q_WORD_MISSING_WITH_G121 _("Q word missing with G12.1") #define NCE_I_J_OR_K_WORDS_MISSING_WITH_G76 _("I J or K words missing with G76") #define NCE_CANNOT_MOVE_ROTARY_AXES_WITH_G76 _("Cannot move rotary axes with G76") #define NCE_MULTIPLE_E_WORDS_ON_ONE_LINE _("Multiple e words on one line") @@ -203,6 +205,7 @@ #define NCE_OUT_OF_MEMORY _("Out of memory") #define NCE_S_WORD_MISSING_WITH_G96 _("S word missing with G96") #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_INPUT _("Queue is not empty after external input") +#define NCE_QUEUE_IS_NOT_EMPTY_AFTER_KINS_SWITCH _("Queue is not empty after Kinematics Switch") #define NCE_ANALOG_INPUT_WITH_WAIT_NOT_IMMEDIATE _("Can't select analog input with wait type != immediate return") #define NCE_ZERO_TIMEOUT_WITH_WAIT_NOT_IMMEDIATE _("Zero timeout with wait type != immediate return") #define NCE_BOTH_DIGITAL_AND_ANALOG_INPUT_SELECTED _("Invalid to select both a digital and an analog input with M66") diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 169e73a8a39..5cfb0b3275e 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -1191,3 +1191,11 @@ StandaloneInterpInternals::StandaloneInterpInternals() : void UPDATE_TAG(const StateTag& /*tag*/){ //Do nothing } + +void ADJUST_KINS_OFFSET(double adjustKinsVar0) +{ + (void)adjustKinsVar0; + printf("saicanon: ADJUST_KINS_OFFSET\n"); + + return; +} diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index a5f45837c99..3aa4c9982db 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1205,6 +1205,17 @@ void ON_RESET() { drop_segments(); } +void ADJUST_KINS_OFFSET(double adjustKinsVar0) +{ + flush_segments(); + + auto adjustKinsOffsetMsg = std::make_unique(); + + adjustKinsOffsetMsg->adjustKinsVar0 = adjustKinsVar0; + + interp_list.append(std::move(adjustKinsOffsetMsg)); +} + CanonConfig_t& get_canon(){ return canon; diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index ff0978fe922..e8b2488018b 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -418,6 +418,8 @@ static EMC_AUX_INPUT_WAIT *emcAuxInputWaitMsg; static int emcAuxInputWaitType = 0; static int emcAuxInputWaitIndex = -1; +static EMC_ADJUST_KINS_OFFSET_DATA *kSwitch_msg; + // commands we compose here static EMC_TASK_PLAN_RUN taskPlanRunCmd; // 16-Aug-1999 FMP //static EMC_TASK_PLAN_INIT taskPlanInitCmd; @@ -1605,6 +1607,10 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) return EMC_TASK_EXEC::WAITING_FOR_MOTION; break; + case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: + return EMC_TASK_EXEC::WAITING_FOR_MOTION_AND_IO; + break; + default: // unrecognized command if (emc_debug & EMC_DEBUG_TASK_ISSUE) { @@ -2427,6 +2433,12 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = 0; break; + case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: + kSwitch_msg = (EMC_ADJUST_KINS_OFFSET_DATA *) cmd; + emcStatus->motion.adjustKinsVar0 = kSwitch_msg->adjustKinsVar0; + retval = emcAdjustKinsOffset(kSwitch_msg->adjustKinsVar0); + break; + default: // unrecognized command if (emc_debug & EMC_DEBUG_TASK_ISSUE) { @@ -2538,6 +2550,10 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) return EMC_TASK_EXEC::DONE; break; + case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: + return EMC_TASK_EXEC::WAITING_FOR_KINS_SWITCH; + break; + default: // unrecognized command if (emc_debug & EMC_DEBUG_TASK_ISSUE) { @@ -2758,6 +2774,17 @@ static int emcTaskExecute(void) } break; + case EMC_TASK_EXEC::WAITING_FOR_KINS_SWITCH: + { + if(emcStatus->motion.trajKinsTypeModified) + { + emcStatus->motion.trajKinsTypeModified = false; + emcTaskPlanSynch(); + emcStatus->task.execState = EMC_TASK_EXEC::DONE; + } + break; + } + case EMC_TASK_EXEC::WAITING_FOR_DELAY: STEPPING_CHECK(); // check if delay has passed diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 482d8bf8afe..4d62173dd39 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -2126,6 +2126,11 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat) r1 = emcJointUpdate(&stat->joint[0], stat->traj.joints); r2 = emcAxisUpdate(&stat->axis[0], stat->traj.axis_mask); r3 = emcTrajUpdate(&stat->traj); + if(stat->trajKinsType != emcmotStatus.kinsType) + { + stat->trajKinsType = emcmotStatus.kinsType; + stat->trajKinsTypeModified = true; + } r4 = emcSpindleUpdate(&stat->spindle[0], stat->traj.spindles); stat->command_type = localMotionCommandType; stat->echo_serial_number = localMotionEchoSerialNumber; @@ -2218,3 +2223,11 @@ int emcGetExternalOffsetApplied(void) { EmcPose emcGetExternalOffsets(void) { return emcmotStatus.eoffset_pose; } + +int emcAdjustKinsOffset(double adjustKinsVar0) +{ + emcmotCommand.command = EMCMOT_ADJUST_KINS_OFFSET_DATA; + emcmotCommand.adjustKinsVar0 = adjustKinsVar0; + + return usrmotWriteEmcmotCommand(&emcmotCommand); +} diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index b191db142ea..2f33b4bbe08 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] -params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) From bfd5a69d64779cb123321592298361e239668233 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 11 Aug 2026 19:04:42 +1000 Subject: [PATCH 02/20] G12.1, G13.1: take the kinematics from motion on every synch The interpreter tracked the kinematics it had selected itself, which is not always the one motion is running. An abort clears the interpreter list, so a G12.1 that was queued but not yet sent is dropped while the interpreter keeps the type it converted. A config that drives motion.switchkins-type from HAL changes the kinematics without the interpreter hearing about it at all. Either way #<_kins_type> reports something that is not running, and the save and restore idiom # = #<_kins_type> G12.1 P3 ( ... ) G12.1 P# puts back the wrong kinematics. Carry the kinematics motion is running up into status and read it back in Interp::synch(), which already runs after an abort and after every completed switch. Task no longer writes the requested value into status, so the field has a single writer and always reports what motion is actually running. --- src/emc/nml_intf/canon.hh | 3 +++ src/emc/rs274ngc/gcodemodule.cc | 1 + src/emc/rs274ngc/rs274ngc_pre.cc | 1 + src/emc/sai/saicanon.cc | 5 +++++ src/emc/task/emccanon.cc | 9 +++++++++ src/emc/task/emctaskmain.cc | 1 - src/emc/task/taskintf.cc | 2 ++ 7 files changed, 21 insertions(+), 1 deletion(-) diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index e49242e3c77..72582f26b29 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -897,6 +897,9 @@ extern int GET_EXTERNAL_MIST(); // Returns the current motion control mode extern CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE(); +// Returns the kinematics type motion is running (G12.1, G13.1) +extern int GET_EXTERNAL_KINS_TYPE(); + // Returns the current motion path-following tolerance extern double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE(); diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index f28092f6e41..226cf8b8980 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -1203,6 +1203,7 @@ void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double /*tolerance*/, int / void SET_MOTION_CONTROL_MODE(double /*tolerance*/) { } void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode) { motion_mode = mode; } CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() { return motion_mode; } +int GET_EXTERNAL_KINS_TYPE() { return 0; } void SET_NAIVECAM_TOLERANCE(double /*tolerance*/) { } #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 16208923637..7b3a4c154e3 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -2073,6 +2073,7 @@ int Interp::synch() _setup.length_units = GET_EXTERNAL_LENGTH_UNIT_TYPE(); _setup.mist = GET_EXTERNAL_MIST(); _setup.plane = GET_EXTERNAL_PLANE(); + _setup.kins_type = GET_EXTERNAL_KINS_TYPE(); _setup.traverse_rate = GET_EXTERNAL_TRAVERSE_RATE(); _setup.feed_override = GET_EXTERNAL_FEED_OVERRIDE_ENABLE(); _setup.adaptive_feed = GET_EXTERNAL_ADAPTIVE_FEED_ENABLE(); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 5cfb0b3275e..41e4c5f4a14 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -777,6 +777,11 @@ extern CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() return _sai._motion_mode; } +extern int GET_EXTERNAL_KINS_TYPE() +{ + return 0; +} + extern void SET_PARAMETER_FILE_NAME(const char *name) { strncpy(_parameter_file_name, name, PARAMETER_FILE_NAME_LENGTH - 1); diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 3aa4c9982db..d02cb487e1f 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -4051,6 +4051,15 @@ CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() return canon.motionMode; } +int GET_EXTERNAL_KINS_TYPE() +{ + // motion publishes the kinematics it is actually running, which is + // not necessarily the one G-code last asked for: an abort can drop a + // queued switch, and the motion.switchkins-type pin can select one + // without the interpreter seeing it + return (int)emcStatus->motion.adjustKinsVar0; +} + double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() { return TO_PROG_LEN(canon.motionTolerance); diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index e8b2488018b..a8213ff1c3e 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -2435,7 +2435,6 @@ static int emcTaskIssueCommand(NMLmsg * cmd) case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: kSwitch_msg = (EMC_ADJUST_KINS_OFFSET_DATA *) cmd; - emcStatus->motion.adjustKinsVar0 = kSwitch_msg->adjustKinsVar0; retval = emcAdjustKinsOffset(kSwitch_msg->adjustKinsVar0); break; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 4d62173dd39..7b0e5d80165 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -2131,6 +2131,8 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat) stat->trajKinsType = emcmotStatus.kinsType; stat->trajKinsTypeModified = true; } + // the kinematics motion is running, whoever selected it + stat->adjustKinsVar0 = emcmotStatus.adjustKinsVar0; r4 = emcSpindleUpdate(&stat->spindle[0], stat->traj.spindles); stat->command_type = localMotionCommandType; stat->echo_serial_number = localMotionEchoSerialNumber; From 0d58a48434d5b6e8428fcd0d9c24952672597e73 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 13:11:24 +1000 Subject: [PATCH 03/20] motion: record the kinematics type only once the switch succeeds handle_kinematicsSwitch() assigned the requested type, published it on motion.kins-type, stored it in the status, and only then asked the module to switch. A module that refuses a type it does not provide goes on running the one it has, so the readout named a kinematics that was not in force, and G12.1 P#<_kins_type> put that wrong number back. Ask first, record after. A refused switch leaves the type, the pin and #<_kins_type> on the kinematics still running, and still raises the motion error. The refusal reached the operator as nothing at all, only a line in the realtime log, which was survivable while switching came from HAL and is not once a G-code block can ask: say which type was refused and which one is still running. The failure message names the type that was asked for rather than the HAL pin, which is not where the request came from when it came from G-code. G12.1 P7 on xyzab_tdr_kins, which provides two types, left motion.kins-type reading 7 while kinstype.is-0 stayed true. It reads 0. --- src/emc/motion/control.c | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 92f854431a0..b721c6ccad8 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -325,10 +325,6 @@ static void handle_kinematicsSwitch(void) { emcmotStatus->adjustKinsVar0 = switchkins_type; if (switchkins_type == requested_type) return; - switchkins_type = requested_type; - hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); - emcmotStatus->adjustKinsVar0 = switchkins_type; - emcmot_joint_t *jointKinsSwitch; double joint_posKinsSwitch[EMCMOT_MAX_JOINTS] = {0,}; /* copy joint position feedback to local array */ @@ -339,13 +335,22 @@ static void handle_kinematicsSwitch(void) { joint_posKinsSwitch[joint_num] = jointKinsSwitch->pos_cmd; } - if (kinematicsSwitch(switchkins_type)) { - rtapi_print_msg(RTAPI_MSG_ERR,"kinematicsSwitch() FAIL<%f>\n", - hal_get_real(emcmot_hal_data->switchkins_type)); + /* a module refuses a type it does not provide and goes on running the + one it has, so nothing is recorded until the switch has happened */ + if (kinematicsSwitch(requested_type)) { + rtapi_print_msg(RTAPI_MSG_ERR,"kinematicsSwitch() FAIL<%d>\n", + requested_type); + reportError(_("kinematics type %d is not provided by this module," + " type %d is still in force"), + requested_type, switchkins_type); SET_MOTION_ERROR_FLAG(1); // abort - return; // no updates for abort + return; // the kinematics in force is unchanged } + switchkins_type = requested_type; + hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); + emcmotStatus->adjustKinsVar0 = switchkins_type; + KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; #ifdef SWITCHKINS_DEBUG From a5352f2c8fa9d8fc57cd653c91186b2c1bd39cfd Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 16:28:10 +1000 Subject: [PATCH 04/20] motion: deprecate selecting the kinematics type from HAL motion.switchkins-type cannot be the general way to choose kinematics. The interpreter never sees it, so a program is read, its limits checked and its path looked ahead in whatever kinematics the interpreter last knew about, which need not be the one that ends up running it. Nothing in the pin can fix that; the interpreter has to be told, which is what G12.1 and G13.1 are for. Motion says so once per session, the first time the pin is used to change the type. A configuration that never switches never sees it, and the G-code route never triggers it. The pin is in a grace period: it keeps working for now, and is meant to go. Both the man page and the switchkins chapter claimed G12.1 and G13.1 write this pin. They do not, and cannot: the configs source it from an analog output that would put its own value back on the next servo cycle. They ask motion directly. --- docs/src/man/man9/motion.9.adoc | 17 +++++++++++------ docs/src/motion/switchkins.adoc | 31 +++++++++++++++++++++++-------- src/emc/motion/control.c | 12 ++++++++++++ 3 files changed, 46 insertions(+), 14 deletions(-) diff --git a/docs/src/man/man9/motion.9.adoc b/docs/src/man/man9/motion.9.adoc index 8b1c78a936e..b68359287d1 100644 --- a/docs/src/man/man9/motion.9.adoc +++ b/docs/src/man/man9/motion.9.adoc @@ -253,15 +253,20 @@ Note: feed-inhibit applies to G-code commands -- not jogs. *motion.switchkins-type* IN float:: Kinematics modules that define the functions kinematicsSwitchable() and kinematicsSwitch() receive the *integer* value of this pin to - select the machine kinematics functions. Extra G-code commands may be + select the machine kinematics functions. Extra G-code commands are required to synchronize task and motion before and after changes to the pin value. - The G-code words *G12.1 P-* and *G13.1* write this pin and synchronize - task and motion themselves, so a program that uses them needs no such - extra commands. + *Deprecated*: the interpreter does not see this pin, so limits and + look ahead go on using the kinematics it last knew about. Use the + G-code words *G12.1 P-* and *G13.1*, which ask motion directly and + synchronize task and motion themselves. Motion reports the + deprecation once, the first time the pin is used to change the + kinematics. The pin is in a grace period: it keeps working for now, + but is meant to be removed in the future. *motion.kins-type* OUT float:: - The kinematics currently selected, echoing the value that was last - applied from *motion.switchkins-type*. + The kinematics currently in force, whether it was selected by + *G12.1*, by *G13.1* or from *motion.switchkins-type*. A kinematics + type the module refuses is not reported here. *motion.teleop-mode* OUT BIT:: Motion mode is teleop (axis coordinate jogging available). *motion.tooloffset.L* OUT FLOAT:: diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index d02633d1515..67eab672180 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -137,11 +137,12 @@ A module providing more than three kinematics types has one === HAL Connections -Switchkins functionality is enabled by the pin -*motion.switchkins-type*, which 'G12.1' and 'G13.1' write directly. -To select a kinstype from HAL instead, source the pin from an analog -output pin like motion.analog-out-03 so that it can be set by M68 -commands. Example: +'G12.1' and 'G13.1' ask motion for a kinstype directly and need no HAL +connection at all. + +A kinstype can also be selected by writing the pin +*motion.switchkins-type*, which is sourced from an analog output pin +like motion.analog-out-03 so that it can be set by M68 commands: [source,hal] ---- @@ -149,6 +150,15 @@ net :kinstype-select <= motion.analog-out-03 net :kinstype-select => motion.switchkins-type ---- +[WARNING] +Selecting the kinstype from HAL is deprecated and motion says so, once, +the first time the pin is used to change it. The interpreter does not +see the pin, so a program is read, its limits checked and its path +looked ahead in whatever kinematics the interpreter last knew about, +which is not necessarily the one that will run it. Use 'G12.1' and +'G13.1'. The pin is in a grace period: it keeps working for now, but is +meant to be removed in the future. + === G-code commands 'G12.1 P-' selects a kinstype and 'G13.1' cancels back to kinstype 0: @@ -191,9 +201,14 @@ description. === M-code commands -A kinstype can also be selected by writing *motion.switchkins-type* -through an analog output pin, which needs the HAL connection shown -above. Kinstype selection is then managed using G-code sequences like: +[WARNING] +This is the deprecated route described under HAL Connections above. It +is documented because existing configurations use it. New ones should +use 'G12.1' and 'G13.1'. + +Writing *motion.switchkins-type* through an analog output pin needs the +HAL connection shown above. Kinstype selection is then managed using +G-code sequences like: [source,ngc] ---- diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index b721c6ccad8..6400d0f9ec2 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -301,6 +301,7 @@ static void handle_kinematicsSwitch(void) { int joint_num; int hal_switchkins_type = 0; static int prev_hal_switchkins_type = 0; + static int said_hal_is_deprecated = 0; int requested_type; if (!kinematicsSwitchable()) return; @@ -318,6 +319,17 @@ static void handle_kinematicsSwitch(void) { emcmotStatus->kinsType = emcmotConfig->kinsType; } else if (hal_switchkins_type != prev_hal_switchkins_type) { requested_type = hal_switchkins_type; + /* Once per session. The pin cannot become the general way to + switch: the interpreter does not see it, so a program is read, + its limits checked and its path looked ahead in whatever + kinematics the interpreter last knew about. */ + if (!said_hal_is_deprecated) { + said_hal_is_deprecated = 1; + reportError(_("motion.switchkins-type is deprecated, use G12.1 and" + " G13.1. Switching kinematics from HAL is invisible" + " to the interpreter, so limits and look ahead go on" + " using the kinematics it last knew about.")); + } } prev_hal_switchkins_type = hal_switchkins_type; From 935e18a2ae502c2989f9e466ad66c2a28b222fe6 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 18:05:45 +1000 Subject: [PATCH 05/20] motion: name the kinematics selection for what it is The G12.1 plumbing arrived from the out-of-tree patch with names that describe nothing. `adjustKinsVar0` is the kinematics type, there is no Var1, and nothing adjusts an offset. `kinsType` is not a type at all: it was a char toggling between 'r' and 's' so the servo cycle could notice that a new request had arrived. The field named like a type was a flag and the field with the opaque name was the type. So: adjustKinsVar0 -> switchkins_type, an int kinsType ('r'/'s' toggle) -> switchkins_seq, a counter trajKinsType -> switchkins_seq in EMC_TRAJ_STAT trajKinsTypeModified -> switchkins_changed in EMC_TRAJ_STAT ADJUST_KINS_OFFSET(double) -> SELECT_KINS_TYPE(int) EMC_ADJUST_KINS_OFFSET_DATA -> EMC_TRAJ_SELECT_KINS EMCMOT_ADJUST_KINS_OFFSET_DATA -> EMCMOT_SELECT_KINS_TYPE emcAdjustKinsOffset() -> emcSelectKinsType() switchkins_type rather than kinsType because EMC_TRAJ_STAT already has kinematics_type, which is the identity/serial/parallel/custom kind and a different thing entirely. switchkins_type is what the HAL pin and switchkins.c already call it. The three status fields were prefixed traj but lived in EMC_MOTION_STAT. They are trajectory status, so they move into EMC_TRAJ_STAT and lose the prefix, which also means EMC_TRAJ_STAT::update() carries them. A counter instead of a two-state toggle keeps the property the toggle had, that asking for the type already in force is still seen as a request, without pretending to be an enum. No G-code, HAL pin or INI name changes. --- src/emc/motion/command.c | 11 +++-------- src/emc/motion/control.c | 10 +++++----- src/emc/motion/motion.h | 16 ++++++++-------- src/emc/nml_intf/canon.hh | 2 +- src/emc/nml_intf/emc.cc | 15 +++++++++------ src/emc/nml_intf/emc.hh | 4 ++-- src/emc/nml_intf/emc_nml.hh | 18 ++++++++++-------- src/emc/nml_intf/emcops.cc | 8 ++++---- src/emc/rs274ngc/gcodemodule.cc | 6 +++--- src/emc/rs274ngc/interp_convert.cc | 2 +- src/emc/sai/saicanon.cc | 6 +++--- src/emc/task/emccanon.cc | 10 +++++----- src/emc/task/emctaskmain.cc | 16 ++++++++-------- src/emc/task/taskintf.cc | 14 +++++++------- 14 files changed, 69 insertions(+), 69 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 1a48585fb5e..22b51ac533f 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -2054,14 +2054,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) axis_set_locking_joint(emcmotCommand->axis, joint_num); break; - case EMCMOT_ADJUST_KINS_OFFSET_DATA: - emcmotConfig->adjustKinsVar0 = emcmotCommand->adjustKinsVar0; - if(emcmotConfig->kinsType == 'r'){ - emcmotConfig->kinsType = 's'; - } - else{ - emcmotConfig->kinsType = 'r'; - } + case EMCMOT_SELECT_KINS_TYPE: + emcmotConfig->switchkins_type = emcmotCommand->switchkins_type; + emcmotConfig->switchkins_seq++; break; default: diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 6400d0f9ec2..52f8d9e0ada 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -314,9 +314,9 @@ static void handle_kinematicsSwitch(void) { hal_switchkins_type = (int)hal_get_real(emcmot_hal_data->switchkins_type); requested_type = switchkins_type; - if (emcmotStatus->kinsType != emcmotConfig->kinsType) { - requested_type = (int)emcmotConfig->adjustKinsVar0; - emcmotStatus->kinsType = emcmotConfig->kinsType; + if (emcmotStatus->switchkins_seq != emcmotConfig->switchkins_seq) { + requested_type = emcmotConfig->switchkins_type; + emcmotStatus->switchkins_seq = emcmotConfig->switchkins_seq; } else if (hal_switchkins_type != prev_hal_switchkins_type) { requested_type = hal_switchkins_type; /* Once per session. The pin cannot become the general way to @@ -334,7 +334,7 @@ static void handle_kinematicsSwitch(void) { prev_hal_switchkins_type = hal_switchkins_type; hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); - emcmotStatus->adjustKinsVar0 = switchkins_type; + emcmotStatus->switchkins_type = switchkins_type; if (switchkins_type == requested_type) return; emcmot_joint_t *jointKinsSwitch; @@ -361,7 +361,7 @@ static void handle_kinematicsSwitch(void) { switchkins_type = requested_type; hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); - emcmotStatus->adjustKinsVar0 = switchkins_type; + emcmotStatus->switchkins_type = switchkins_type; KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 9a7bf7959a7..351e387f0b1 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -174,10 +174,9 @@ extern "C" { 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_ADJUST_KINS_OFFSET_DATA, /* set the offset in kins (G12.1) */ + EMCMOT_SET_AXIS_JERK_LIMIT, /* set the max axis jerk */ + EMCMOT_SELECT_KINS_TYPE, /* select the switchkins type (G12.1) */ EMCMOT_SET_SPINDLE_PARAMS, /* One command to set all spindle params */ } cmd_code_t; @@ -273,7 +272,7 @@ extern "C" { double ext_offset_acc; /* acceleration for an external axis offset */ struct state_tag_t tag; - double adjustKinsVar0; + int switchkins_type; /* switchkins type requested by G12.1 */ } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars @@ -672,8 +671,8 @@ Suggestion: Split this in to an Error and a Status flag register.. int stepping; bool jogging_active; - char kinsType; - double adjustKinsVar0; + int switchkins_seq; /* echoes the config counter once acted on */ + int switchkins_type; /* switchkins type now in force */ } emcmot_status_t; /********************************* @@ -746,8 +745,9 @@ Suggestion: Split this in to an Error and a Status flag register.. int inhibit_probe_jog_error; int inhibit_probe_home_error; - double adjustKinsVar0; - char kinsType; + int switchkins_type; /* switchkins type requested by G12.1 */ + int switchkins_seq; /* bumped per request, so a repeat of + the same type is still seen */ } emcmot_config_t; /* error structure - lockfree MPSC ring buffer. See emcmotutil.c. */ diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 72582f26b29..4889283769f 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -1074,6 +1074,6 @@ extern EmcPose GET_EXTERNAL_OFFSETS(); extern void UPDATE_TAG(const StateTag& tag); // adjust kins offset (G12.1 kinematics switch) -extern void ADJUST_KINS_OFFSET(double adjustKinsVar0); +extern void SELECT_KINS_TYPE(int switchkins_type); #endif /* ifndef CANON_HH */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 848db4990da..a2e8e65cb7e 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -296,8 +296,8 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_OFFSET_TYPE: ((EMC_TRAJ_SET_OFFSET *) buffer)->update(cms); break; - case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: - ((EMC_ADJUST_KINS_OFFSET_DATA *) buffer)->update(cms); + case EMC_TRAJ_SELECT_KINS_TYPE: + ((EMC_TRAJ_SELECT_KINS *) buffer)->update(cms); break; case EMC_TRAJ_SET_G5X_TYPE: ((EMC_TRAJ_SET_G5X *) buffer)->update(cms); @@ -523,8 +523,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_MODE"; case EMC_TRAJ_SET_OFFSET_TYPE: return "EMC_TRAJ_SET_OFFSET"; - case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: - return "EMC_ADJUST_KINS_OFFSET_DATA"; + case EMC_TRAJ_SELECT_KINS_TYPE: + return "EMC_TRAJ_SELECT_KINS"; case EMC_TRAJ_SET_G5X_TYPE: return "EMC_TRAJ_SET_G5X"; case EMC_TRAJ_SET_G92_TYPE: @@ -1597,10 +1597,10 @@ void EMC_TRAJ_SET_OFFSET::update(CMS * cms) } // cppcheck-suppress duplInheritedMember -void EMC_ADJUST_KINS_OFFSET_DATA::update(CMS * cms) +void EMC_TRAJ_SELECT_KINS::update(CMS * cms) { EMC_TRAJ_CMD_MSG::update(cms); - cms->update(adjustKinsVar0); + cms->update(switchkins_type); } /* @@ -1738,6 +1738,9 @@ void EMC_TRAJ_STAT::update(CMS * cms) cms->update(feed_override_enabled); cms->update(adaptive_feed_enabled); cms->update(feed_hold_enabled); + cms->update(switchkins_type); + cms->update(switchkins_seq); + cms->update(switchkins_changed); } /* diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 91688da73c1..c2e83d1d379 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -112,7 +112,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) -#define EMC_ADJUST_KINS_OFFSET_DATA_TYPE ((NMLTYPE) 289) +#define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) // EMC_MOTION aggregate class type declaration @@ -462,7 +462,7 @@ int emcSetupArcBlends(int arcBlendEnable, int emcSetProbeErrorInhibit(int j_inhibit, int h_inhibit); int emcGetExternalOffsetApplied(void); EmcPose emcGetExternalOffsets(void); -extern int emcAdjustKinsOffset(double adjustKinsVar0); +extern int emcSelectKinsType(int switchkins_type); extern int emcUpdate(EMC_STAT * stat); // full EMC status diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index bb88a94ea75..9ed92f6b3a3 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -960,14 +960,14 @@ class EMC_TRAJ_RIGID_TAP:public EMC_TRAJ_CMD_MSG { double vel, ini_maxvel, acc, scale, ini_maxjerk; }; -class EMC_ADJUST_KINS_OFFSET_DATA:public EMC_TRAJ_CMD_MSG { +class EMC_TRAJ_SELECT_KINS:public EMC_TRAJ_CMD_MSG { public: - EMC_ADJUST_KINS_OFFSET_DATA():EMC_TRAJ_CMD_MSG(EMC_ADJUST_KINS_OFFSET_DATA_TYPE, - sizeof(EMC_ADJUST_KINS_OFFSET_DATA)), - adjustKinsVar0(0.0) + EMC_TRAJ_SELECT_KINS():EMC_TRAJ_CMD_MSG(EMC_TRAJ_SELECT_KINS_TYPE, + sizeof(EMC_TRAJ_SELECT_KINS)), + switchkins_type(0) {}; - double adjustKinsVar0; + int switchkins_type; // For internal NML/CMS use only. // Sub-class update() calls base-class update() @@ -1039,6 +1039,11 @@ class EMC_TRAJ_STAT:public EMC_TRAJ_STAT_MSG { //bool spindle_override_enabled; moved to SPINDLE_STAT bool adaptive_feed_enabled; bool feed_hold_enabled; + + int switchkins_type; // switchkins type now in force + int switchkins_seq; // motion's request counter, echoed once seen + bool switchkins_changed; // a switch landed, task has yet to synch + StateTag tag; }; @@ -1182,9 +1187,6 @@ class EMC_MOTION_STAT:public EMC_MOTION_STAT_MSG { bool jogging_active; uint64_t heartbeat; // motion controller's heartbeat counter - char trajKinsType; - bool trajKinsTypeModified; - double adjustKinsVar0; }; // declarations for EMC_TASK classes diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index 49868ce1047..437c6d08019 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -94,6 +94,9 @@ EMC_TRAJ_STAT::EMC_TRAJ_STAT() feed_override_enabled(OFF), adaptive_feed_enabled(OFF), feed_hold_enabled(OFF), + switchkins_type(0), + switchkins_seq(0), + switchkins_changed(false), tag() { } @@ -111,10 +114,7 @@ EMC_MOTION_STAT::EMC_MOTION_STAT() eoffset_pose{}, numExtraJoints(0), jogging_active(0), - heartbeat(0), - trajKinsType(0), - trajKinsTypeModified(false), - adjustKinsVar0(0.0) + heartbeat(0) { } diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 226cf8b8980..6ead6b3b746 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -890,10 +890,10 @@ void ON_RESET() {} void PALLET_SHUTTLE() {} void SELECT_TOOL(int tool) {selected_tool = tool;} void UPDATE_TAG(const StateTag& /*tag*/) {} -void ADJUST_KINS_OFFSET(double adjustKinsVar0) +void SELECT_KINS_TYPE(int switchkins_type) { - (void)adjustKinsVar0; - printf("gcodemodule: ADJUST_KINS_OFFSET\n"); + (void)switchkins_type; + printf("gcodemodule: SELECT_KINS_TYPE\n"); return; } diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 1c3394812bc..e70db391422 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6530,7 +6530,7 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 CHKS((kins_type < 0), _("G12.1 requires a non-negative P word")); - ADJUST_KINS_OFFSET((double)kins_type); + SELECT_KINS_TYPE(kins_type); settings->kins_type = kins_type; return INTERP_OK; } diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 41e4c5f4a14..73af2751dd4 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -1197,10 +1197,10 @@ void UPDATE_TAG(const StateTag& /*tag*/){ //Do nothing } -void ADJUST_KINS_OFFSET(double adjustKinsVar0) +void SELECT_KINS_TYPE(int switchkins_type) { - (void)adjustKinsVar0; - printf("saicanon: ADJUST_KINS_OFFSET\n"); + (void)switchkins_type; + printf("saicanon: SELECT_KINS_TYPE\n"); return; } diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index d02cb487e1f..6fb57457464 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1205,15 +1205,15 @@ void ON_RESET() { drop_segments(); } -void ADJUST_KINS_OFFSET(double adjustKinsVar0) +void SELECT_KINS_TYPE(int switchkins_type) { flush_segments(); - auto adjustKinsOffsetMsg = std::make_unique(); + auto selectKinsMsg = std::make_unique(); - adjustKinsOffsetMsg->adjustKinsVar0 = adjustKinsVar0; + selectKinsMsg->switchkins_type = switchkins_type; - interp_list.append(std::move(adjustKinsOffsetMsg)); + interp_list.append(std::move(selectKinsMsg)); } @@ -4057,7 +4057,7 @@ int GET_EXTERNAL_KINS_TYPE() // not necessarily the one G-code last asked for: an abort can drop a // queued switch, and the motion.switchkins-type pin can select one // without the interpreter seeing it - return (int)emcStatus->motion.adjustKinsVar0; + return emcStatus->motion.traj.switchkins_type; } double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index a8213ff1c3e..5e41127a5a2 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -418,7 +418,7 @@ static EMC_AUX_INPUT_WAIT *emcAuxInputWaitMsg; static int emcAuxInputWaitType = 0; static int emcAuxInputWaitIndex = -1; -static EMC_ADJUST_KINS_OFFSET_DATA *kSwitch_msg; +static EMC_TRAJ_SELECT_KINS *kSwitch_msg; // commands we compose here static EMC_TASK_PLAN_RUN taskPlanRunCmd; // 16-Aug-1999 FMP @@ -1607,7 +1607,7 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) return EMC_TASK_EXEC::WAITING_FOR_MOTION; break; - case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: + case EMC_TRAJ_SELECT_KINS_TYPE: return EMC_TASK_EXEC::WAITING_FOR_MOTION_AND_IO; break; @@ -2433,9 +2433,9 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = 0; break; - case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: - kSwitch_msg = (EMC_ADJUST_KINS_OFFSET_DATA *) cmd; - retval = emcAdjustKinsOffset(kSwitch_msg->adjustKinsVar0); + case EMC_TRAJ_SELECT_KINS_TYPE: + kSwitch_msg = (EMC_TRAJ_SELECT_KINS *) cmd; + retval = emcSelectKinsType(kSwitch_msg->switchkins_type); break; default: @@ -2549,7 +2549,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) return EMC_TASK_EXEC::DONE; break; - case EMC_ADJUST_KINS_OFFSET_DATA_TYPE: + case EMC_TRAJ_SELECT_KINS_TYPE: return EMC_TASK_EXEC::WAITING_FOR_KINS_SWITCH; break; @@ -2775,9 +2775,9 @@ static int emcTaskExecute(void) case EMC_TASK_EXEC::WAITING_FOR_KINS_SWITCH: { - if(emcStatus->motion.trajKinsTypeModified) + if(emcStatus->motion.traj.switchkins_changed) { - emcStatus->motion.trajKinsTypeModified = false; + emcStatus->motion.traj.switchkins_changed = false; emcTaskPlanSynch(); emcStatus->task.execState = EMC_TASK_EXEC::DONE; } diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 7b0e5d80165..09b494d183c 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -2126,13 +2126,13 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat) r1 = emcJointUpdate(&stat->joint[0], stat->traj.joints); r2 = emcAxisUpdate(&stat->axis[0], stat->traj.axis_mask); r3 = emcTrajUpdate(&stat->traj); - if(stat->trajKinsType != emcmotStatus.kinsType) + if(stat->traj.switchkins_seq != emcmotStatus.switchkins_seq) { - stat->trajKinsType = emcmotStatus.kinsType; - stat->trajKinsTypeModified = true; + stat->traj.switchkins_seq = emcmotStatus.switchkins_seq; + stat->traj.switchkins_changed = true; } // the kinematics motion is running, whoever selected it - stat->adjustKinsVar0 = emcmotStatus.adjustKinsVar0; + stat->traj.switchkins_type = emcmotStatus.switchkins_type; r4 = emcSpindleUpdate(&stat->spindle[0], stat->traj.spindles); stat->command_type = localMotionCommandType; stat->echo_serial_number = localMotionEchoSerialNumber; @@ -2226,10 +2226,10 @@ EmcPose emcGetExternalOffsets(void) { return emcmotStatus.eoffset_pose; } -int emcAdjustKinsOffset(double adjustKinsVar0) +int emcSelectKinsType(int switchkins_type) { - emcmotCommand.command = EMCMOT_ADJUST_KINS_OFFSET_DATA; - emcmotCommand.adjustKinsVar0 = adjustKinsVar0; + emcmotCommand.command = EMCMOT_SELECT_KINS_TYPE; + emcmotCommand.switchkins_type = switchkins_type; return usrmotWriteEmcmotCommand(&emcmotCommand); } From 1c4b5a3f34237abec5760ce8c5a75147e03dacc7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 16:57:15 +1000 Subject: [PATCH 06/20] configs: select kinematics with G12.1 in the switchkins comp sims The four sim configs whose kinematics components now use the switchkins core chose their kinematics by writing motion.switchkins-type through an analog output, the route motion has just deprecated. Each of them would have met the user with the deprecation warning the first time they pressed a kinematics button. The M428, M429 and M430 remaps, the TWP wrappers behind G53.1, G53.3, G53.6 and G69, the abort handler and remap.py now use G12.1 and G13.1. That drops the M66 sync either side of every switch, the test that the HAL pin exists at all, and the #5399 clobber each M66 costs, since G12.1 and G13.1 synchronise interpreter and motion themselves. The check that the switch took reads #<_kins_type> instead of the pin. millturn keeps the M66 at the end of M428 and M429. That one is not there for the switch: M128 and M129 change the axis limits from a Tcl script, which reaches motion through inihal, so read-ahead has to stop until the new limits have landed. The vismach guis for the two trsrn configs were reading the value requested through the analog output. They now take motion.kins-type, which is the kinematics actually in force. Eight other sim config directories still select kinematics from HAL: bridgemill, table-rotary-tilting, hexapod-sim, melfa-sim, puma, and the three copies of scara. They are untouched here, and still work. --- .../vismach/5axis/table-dual-rotary/README | 3 --- .../table-dual-rotary/remap_subs/428remap.ngc | 21 +++++-------------- .../table-dual-rotary/remap_subs/429remap.ngc | 17 +++------------ .../5axis/table-dual-rotary/xyzab-tdr.ini | 10 ++++----- .../python/remap.py | 2 +- .../remap_subs/428remap.ngc | 17 +++------------ .../remap_subs/429remap.ngc | 17 +++------------ .../remap_subs/430remap.ngc | 17 +++------------ .../remap_subs/g531remap.ngc | 2 +- .../remap_subs/g533remap.ngc | 2 +- .../remap_subs/g536remap.ngc | 2 +- .../remap_subs/g69remap.ngc | 2 +- .../remap_subs/on_abort_with_twp_reset.ngc | 2 +- .../xyzacb-trsrn_twp/xyzacb-trsrn.ini | 10 ++++----- .../xyzbca-trsrn_twp/xyzbca-trsrn.ini | 10 ++++----- .../sim/axis/vismach/millturn/millturn.ini | 1 - .../sim/axis/vismach/millturn/millturn.txt | 5 ++--- .../vismach/millturn/remap_subs/428remap.ngc | 19 ++++------------- .../vismach/millturn/remap_subs/429remap.ngc | 19 ++++------------- 19 files changed, 45 insertions(+), 133 deletions(-) diff --git a/configs/sim/axis/vismach/5axis/table-dual-rotary/README b/configs/sim/axis/vismach/5axis/table-dual-rotary/README index 0a9f1130e42..29e0a4c88a4 100644 --- a/configs/sim/axis/vismach/5axis/table-dual-rotary/README +++ b/configs/sim/axis/vismach/5axis/table-dual-rotary/README @@ -28,9 +28,6 @@ For proper tool-path preview RELOAD THE CGODE after startup and after changing o *********************************************** Note: IMPORTANT ini file requirements: -[HAL] -HALCMD = net :kinstype-select <= motion.analog-out-0N => motion.switchkins-type - [RS274NGC] SUBROUTINE_PATH = ./remap_subs REMAP = M428 modalgroup=10 ngc=428remap diff --git a/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/428remap.ngc b/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/428remap.ngc index 46e2d01ba5b..062edba961e 100644 --- a/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ -;M428 by remap: kinstype==1 (xyzac,xyzbc) (note: sparm=identityfirst) +;M428 by remap: kinstype==1 (xyzab-tdr kinematics) o<428remap>sub - # = 1 ; xyzac,xyzbc - # = 3 ; set N as required: motion.analog-out-0N + # = 1 ; xyzab-tdr -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/429remap.ngc b/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/429remap.ngc index 3fa610c8ee0..ff81c491a6f 100644 --- a/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: kinstype==0 Identity kinematics o<429remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini b/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini index 339ed834a8e..bd6dc79e82a 100644 --- a/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini +++ b/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini @@ -25,8 +25,8 @@ SUBROUTINE_PATH = ./remap_subs [KINS] #NOTE: -# switchkins-type == 0 is identity kins -# switchkins-type == 1 is xyzab-tdr-kins +# kinstype 0 is identity kins +# kinstype 1 is xyzab-tdr-kins KINEMATICS = xyzab_tdr_kins JOINTS = 5 @@ -36,8 +36,6 @@ KINEMATICS = xyzab_tdr_kins HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = xyzab-tdr-postgui.hal -# net for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # Values '(x,z)-offsets' for geometric offset of the rotary-assembly and the # values '(x,y,z)-rot-point' that describe the position of the @@ -76,8 +74,8 @@ HALCMD = sets :x-offset -20 HALCMD = sets :z-offset -10 [HALUI] -# M429:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M428:xyzab-tdr kins (motion.switchkins-type==1) +# M429:identity kins (kinstype 0, startupDEFAULT) +# M428:xyzab-tdr kins (kinstype 1) MDI_COMMAND = M429 MDI_COMMAND = M428 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py index c8b15c9c4e9..f4f9506a846 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py @@ -871,7 +871,7 @@ def g53x_core(self): # switch to the dedicated TWP work offsets self.execute("G59", lineno()) # activate TOOL kinematics - self.execute("M68 E3 Q2") + self.execute("G12.1 P2") if (x,y,z) != (None,None,None): log.debug('G53.3 called') self.execute("G0 X%s Y%s Z%s %s%f %s%f" % (x, y, z, joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/428remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/428remap.ngc index bcd3c730a1f..381a6116adf 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: kinstype==0 (IDENTITY kinematics) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/429remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/429remap.ngc index 0d14ad1bf82..d1b54b5250d 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: kinstype==1 TCP kinematics o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/430remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/430remap.ngc index 55fbf966e11..5f726a6df12 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: kinstype==2 Tool kinematics o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M430:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc index b7c27d221d3..4b2fd293c61 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc @@ -6,7 +6,7 @@ o100 if [EXISTS [#

]] o100 else #

= 0 ;if no P word has been passed we use the default (0) o100 endif -M68 E3 Q0 ;switch to identity kinematic +G13.1 ;back to identity kinematic M66 L0 E0 M530 P#

;orient the spindle with P word M66 L0 E0 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc index c25356b27a8..16d9687cbe8 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc @@ -3,7 +3,7 @@ osub M66 L0 E0 ;force sync, stop read ahead o100 if [[EXISTS [#]] AND [EXISTS [#]] AND [EXISTS [#]]] - M68 E3 Q0 ;switch to identity kinematic + G13.1 ;back to identity kinematic o100 else (abort, G53.3: X,Y and Z words are required) ;it is an error if X,Y or Z word is missing o100 endif diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc index 718a572afae..a8b628a930c 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc @@ -6,7 +6,7 @@ o100 if [EXISTS [#

]] o100 else #

= 0 ;if no P word has been passed we use the default (0) o100 endif -M68 E3 Q1 ;switch to tcp kinematic +G12.1 P1 ;switch to tcp kinematic M66 L0 E0 M530 P#

;orient the spindle with P word M66 L0 E0 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc index fd37ea30837..9efd1b7db29 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc @@ -3,7 +3,7 @@ osub M66 L0 E0 ; force sync, stop read ahead M469 ; call the python G69_core code -M68 E3 Q0 ; switch to identity kins +G13.1 ; back to identity kins M68 E2 Q0 ; reset twp-state to 'undefined' (0) G54 ; switch to G54 M66 L0 E0 ; force sync, stop read ahead diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc index 492552977e2..1cbf3d41db7 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc @@ -7,7 +7,7 @@ o sub ;(msg, on_abort START) M68 E2 Q0 ; reset twp-state to 'undefined' (0) -M68 E3 Q0 ; set IDENTITY kins +G13.1 ; back to identity kins G64 P0.01 ; reset the toolpath tolerance as this sometimes gets set to zero on estop events G54 ; switch to G54 (msg, on_abort END) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 0e430c12691..06b9cd5d23f 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini @@ -80,8 +80,6 @@ POSTGUI_HALFILE = xyzacb-trsrn_postgui.hal # signal reflecting twp states (0=undefined, 1=defined, 2=active) HALCMD = net twp-status <= motion.analog-out-02 -# connection required for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzacb_trsrn_kins.tool-offset-z @@ -124,7 +122,7 @@ HALCMD = net :rotary-b joint.4.pos-fb xyzacb-trsrn-gui.rot HALCMD = net :rotary-c joint.5.pos-fb xyzacb-trsrn-gui.rotary_c HALCMD = net :tool-diam halui.tool.diameter xyzacb-trsrn-gui.tool_diameter HALCMD = net :tool-offset xyzacb-trsrn-gui.tool_length -HALCMD = net :kinstype-select xyzacb-trsrn-gui.kinstype_select +HALCMD = net :kinstype-current motion.kins-type xyzacb-trsrn-gui.kinstype_select HALCMD = net :nutation-angle xyzacb-trsrn-gui.nutation_angle HALCMD = net :pivot-y xyzacb-trsrn-gui.pivot_y HALCMD = net :pivot-z xyzacb-trsrn-gui.pivot_z @@ -155,9 +153,9 @@ HALCMD = net twp-is-active xyzacb-trsrn-gui.twp [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst -# M428:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M429: tcp kins (motion.switchkins-type==1) -# M430: tool kins (motion.switchkins-type==2) +# M428:identity kins (kinstype 0, startupDEFAULT) +# M429: tcp kins (kinstype 1) +# M430: tool kins (kinstype 2) MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini index 44e6144e653..d9ae382fefc 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini @@ -77,8 +77,6 @@ POSTGUI_HALFILE = xyzbca-trsrn_postgui.hal # signal reflecting twp states (0=undefined, 1=defined, 2=active) HALCMD = net twp-status <= motion.analog-out-02 -# connection required for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzbca_trsrn_kins.tool-offset-z @@ -121,7 +119,7 @@ HALCMD = net :rotary-b joint.4.pos-fb xyzbca-trsrn-gui.rot HALCMD = net :rotary-c joint.5.pos-fb xyzbca-trsrn-gui.rotary_c HALCMD = net :tool-diam halui.tool.diameter xyzbca-trsrn-gui.tool_diameter HALCMD = net :tool-offset xyzbca-trsrn-gui.tool_length -HALCMD = net :kinstype-select xyzbca-trsrn-gui.kinstype_select +HALCMD = net :kinstype-current motion.kins-type xyzbca-trsrn-gui.kinstype_select HALCMD = net :nutation-angle xyzbca-trsrn-gui.nutation_angle HALCMD = net :pivot-x xyzbca-trsrn-gui.pivot_x HALCMD = net :pivot-z xyzbca-trsrn-gui.pivot_z @@ -152,9 +150,9 @@ HALCMD = net twp-is-active xyzbca-trsrn-gui.twp [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst -# M428:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M429: tcp kins (motion.switchkins-type==1) -# M430: tool kins (motion.switchkins-type==2) +# M428:identity kins (kinstype 0, startupDEFAULT) +# M429: tcp kins (kinstype 1) +# M430: tool kins (kinstype 2) MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 diff --git a/configs/sim/axis/vismach/millturn/millturn.ini b/configs/sim/axis/vismach/millturn/millturn.ini index 57776eeb38c..575947d9dce 100644 --- a/configs/sim/axis/vismach/millturn/millturn.ini +++ b/configs/sim/axis/vismach/millturn/millturn.ini @@ -14,7 +14,6 @@ JOINTS= 4 HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = millturn.hal -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = millturn-postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/millturn/millturn.txt b/configs/sim/axis/vismach/millturn/millturn.txt index b6cbe143a03..29b9960d6e4 100644 --- a/configs/sim/axis/vismach/millturn/millturn.txt +++ b/configs/sim/axis/vismach/millturn/millturn.txt @@ -7,9 +7,8 @@ For additional information see the README in the millturn folder. 2) pyvcp buttons are provided to switch between mill and turn kinematics. The buttons issue remapped commands M428,M429. These commands -a) set the motion.switchkins-type pin and -b) force a synchronization using a motion input read command. -c) set softlimits according to values set in millturn.ini [AXIS_X] and [AXIS_Z] section. +a) select the kinematics with G12.1, which synchronizes interpreter and motion itself. +b) set softlimits according to values set in millturn.ini [AXIS_X] and [AXIS_Z] section. 3) when set for mill, default assignments are: diff --git a/configs/sim/axis/vismach/millturn/remap_subs/428remap.ngc b/configs/sim/axis/vismach/millturn/remap_subs/428remap.ngc index ca6225fb421..63a2d118a6d 100644 --- a/configs/sim/axis/vismach/millturn/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/millturn/remap_subs/428remap.ngc @@ -1,29 +1,18 @@ ;M428 by remap: select mill kins o<428remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 0 ; mill -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value + G12.1 P# ; select kinstype, syncs interp and motion M128 ; switch limits G10 L2 P7 X-290 Y0 Z-160 A0 ; reset home offset G59.1 ; activate home offset - M66 E0 L0 ; force synch + M66 E0 L0 ; force synch, M128 changed the limits ;(debug, M428: mill) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 0]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/millturn/remap_subs/429remap.ngc b/configs/sim/axis/vismach/millturn/remap_subs/429remap.ngc index 26207430a88..7be809a0d3e 100644 --- a/configs/sim/axis/vismach/millturn/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/millturn/remap_subs/429remap.ngc @@ -1,29 +1,18 @@ ;M429 by remap: select turn kins o<429remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 1 ; turn kins -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]FEATURE==8) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value + G12.1 P# ; select kinstype, syncs interp and motion M129 ; switch limits G10 L2 P8 X-160 Y0 Z-290 A0 ; reset home offset G59.2 ; activate home offset - M66 E0 L0 ; force synch + M66 E0 L0 ; force synch, M129 changed the limits ;(debug, M429: turn) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 1]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub From 053b08a9793c3d03e6be4cad64a3e55e5a5e1ec6 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 17:16:13 +1000 Subject: [PATCH 07/20] configs: select kinematics with G12.1 in the remaining switchkins sims The rest of the sim configs that shipped with switchkins chose their kinematics by writing motion.switchkins-type through an analog output, which motion now reports as deprecated: bridgemill, table-rotary-tilting, hexapod-sim, melfa-sim, puma and the three copies of scara. Same change as the comp sims got. The M428, M429 and M430 remaps use G12.1 and G13.1, which drops the M66 sync either side of every switch, the test for the hal pin, and the #5399 clobber each M66 costs. The check that the switch took reads #<_kins_type>. The [HAL] net from motion.analog-out-03 goes with them, and the two halshow watch lists follow motion.kins-type instead of the pin that used to drive it. No sim config selects kinematics from HAL now. --- .../axis/vismach/5axis/bridgemill/5axis.ini | 1 - .../5axis/bridgemill/remap_subs/428remap.ngc | 17 +++-------------- .../5axis/bridgemill/remap_subs/429remap.ngc | 17 +++-------------- .../5axis/bridgemill/remap_subs/430remap.ngc | 17 +++-------------- .../vismach/5axis/table-rotary-tilting/README | 3 --- .../remap_subs/428remap.ngc | 17 +++-------------- .../remap_subs/429remap.ngc | 17 +++-------------- .../remap_subs/430remap.ngc | 17 +++-------------- .../table-rotary-tilting/switchkins.halshow | 3 +-- .../5axis/table-rotary-tilting/xyzac-trt.ini | 12 +++++------- .../5axis/table-rotary-tilting/xyzac-trt.txt | 8 +++----- .../5axis/table-rotary-tilting/xyzbc-trt.ini | 12 +++++------- .../5axis/table-rotary-tilting/xyzbc-trt.txt | 8 +++----- .../sim/axis/vismach/hexapod-sim/hexapod.ini | 1 - .../hexapod-sim/remap_subs/428remap.ngc | 17 +++-------------- .../hexapod-sim/remap_subs/429remap.ngc | 17 +++-------------- .../hexapod-sim/remap_subs/430remap.ngc | 17 +++-------------- .../vismach/melfa-sim/melfa-sim-genser/README | 8 ++++---- .../melfa-sim-genser/melfa-sim-genser.ini | 1 - .../vismach/melfa-sim/melfa-sim-three21/README | 8 ++++---- .../melfa-sim/melfa-sim-three21/melfa_321.ini | 1 - .../vismach/melfa-sim/remap_subs/428remap.ngc | 18 +++--------------- .../vismach/melfa-sim/remap_subs/429remap.ngc | 18 +++--------------- .../vismach/melfa-sim/remap_subs/430remap.ngc | 18 +++--------------- configs/sim/axis/vismach/puma/puma.ini | 1 - configs/sim/axis/vismach/puma/puma560.halshow | 2 +- configs/sim/axis/vismach/puma/puma560.ini | 1 - configs/sim/axis/vismach/puma/puma560.txt | 8 ++++---- configs/sim/axis/vismach/puma/puma560_uvw.ini | 1 - configs/sim/axis/vismach/puma/puma_cube.ini | 1 - .../axis/vismach/puma/remap_subs/428remap.ngc | 17 +++-------------- .../axis/vismach/puma/remap_subs/429remap.ngc | 17 +++-------------- .../axis/vismach/puma/remap_subs/430remap.ngc | 17 +++-------------- .../axis/vismach/scara/remap_subs/428remap.ngc | 17 +++-------------- .../axis/vismach/scara/remap_subs/429remap.ngc | 17 +++-------------- .../axis/vismach/scara/remap_subs/430remap.ngc | 17 +++-------------- configs/sim/axis/vismach/scara/scara.ini | 1 - .../non-trivial/scara/remap_subs/428remap.ngc | 17 +++-------------- .../non-trivial/scara/remap_subs/429remap.ngc | 17 +++-------------- .../non-trivial/scara/remap_subs/430remap.ngc | 17 +++-------------- .../non-trivial/scara/remap_subs/428remap.ngc | 17 +++-------------- .../non-trivial/scara/remap_subs/429remap.ngc | 17 +++-------------- .../non-trivial/scara/remap_subs/430remap.ngc | 17 +++-------------- 43 files changed, 102 insertions(+), 390 deletions(-) diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini index 8ca0552431a..38e706fac22 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini @@ -41,7 +41,6 @@ CYCLE_TIME = 0.010 HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = 5axisgui.hal -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = 5axis_postgui.hal [HALUI] diff --git a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/428remap.ngc b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/428remap.ngc index 4ab3aaf922d..e9529f6d0f8 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype=0 genhexkins o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/429remap.ngc b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/429remap.ngc index 54726d37a6c..0291e69889d 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 Identity kinematics o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/430remap.ngc b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/430remap.ngc index 7586236a003..886fe727740 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 userk kins o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/README b/configs/sim/axis/vismach/5axis/table-rotary-tilting/README index 4166a10fc2a..b85e076dd16 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/README +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/README @@ -17,9 +17,6 @@ Demonstrations: *********************************************** Note: IMPORTANT ini file requirements: -[HAL] -HALCMD = net :kinstype-select <= motion.analog-out-0N => motion.switchkins-type - [RS274NGC] SUBROUTINE_PATH = ./remap_subs REMAP = M428 modalgroup=10 ngc=428remap diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/428remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/428remap.ngc index 46e2d01ba5b..5255b230004 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: kinstype==1 (xyzac,xyzbc) (note: sparm=identityfirst) o<428remap>sub # = 1 ; xyzac,xyzbc - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/429remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/429remap.ngc index 3fa610c8ee0..be20d5b06b7 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: kinstype==0 Identity kinematics o<429remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/430remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/430remap.ngc index 65a82221335..6679a3080da 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: kinstype==2 userk kins o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M430:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/switchkins.halshow b/configs/sim/axis/vismach/5axis/table-rotary-tilting/switchkins.halshow index ede94bdc5bd..014d9531eb7 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/switchkins.halshow +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/switchkins.halshow @@ -1,5 +1,4 @@ -pin+motion.analog-out-03 -pin+motion.switchkins-type +pin+motion.kins-type pin+joint.0.pos-cmd pin+joint.1.pos-cmd diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini index 94650238225..9c2bc00e33e 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini @@ -38,8 +38,8 @@ SUBROUTINE_PATH = ./remap_subs [KINS] #NOTE: for backwrds compatibility !!!!!!!!!!!!!!!!!!! -# default switchkins-type == 0 is xyzac-trt-kins -# here switchkins-type == 0 is identity kins +# default kinstype 0 is xyzac-trt-kins +# here kinstype 0 is identity kins KINEMATICS = xyzac-trt-kins sparm=identityfirst JOINTS = 5 @@ -48,8 +48,6 @@ KINEMATICS = xyzac-trt-kins sparm=identityfirst HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = switchkins_postgui.hal -# net for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # vismach xyzac-trt-gui items HALCMD = loadusr -W ./xyzac-trt-gui.py @@ -73,9 +71,9 @@ HALCMD = setp xyzac-trt-kins.conventional-directions 0 [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst -# M429:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M428:xyzac kins (motion.switchkins-type==1) -# M430:userk kins (motion.switchkins-type==2) +# M429:identity kins (kinstype 0, startupDEFAULT) +# M428:xyzac kins (kinstype 1) +# M430:userk kins (kinstype 2) MDI_COMMAND = M429 MDI_COMMAND = M428 MDI_COMMAND = M430 diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.txt b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.txt index acdbdebe6a5..7bc132e3159 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.txt +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.txt @@ -6,11 +6,9 @@ Uses remapped user m codes for kins switch: M428: XYZAC (TCP) M430: userk Kinematics -A hal net is required to connect the -analog out pin N, Example (for N=3): - - net :kinstype-select <= motion.analog-out-03 - net :kinstype-select => motion.switchkins-type +The kinematics type is selected with +G12.1 and G13.1, no hal connection is +required. Hal Input pins: xyzac-trt-kins.y-offset diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini index 5a1524b7e2d..45d780a251c 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini @@ -38,8 +38,8 @@ SUBROUTINE_PATH = ./remap_subs [KINS] #NOTE: for backwrds compatibility !!!!!!!!!!!!!!!!!!! -# default switchkins-type == 0 is xyzbc-trt-kins -# here switchkins-type == 0 is identity kins +# default kinstype 0 is xyzbc-trt-kins +# here kinstype 0 is identity kins KINEMATICS = xyzbc-trt-kins sparm=identityfirst JOINTS = 5 @@ -48,8 +48,6 @@ KINEMATICS = xyzbc-trt-kins sparm=identityfirst HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = switchkins_postgui.hal -# net for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # vismach xyzbc-trt-gui items HALCMD = loadusr -W ./xyzbc-trt-gui.py @@ -73,9 +71,9 @@ HALCMD = setp xyzbc-trt-kins.conventional-directions 0 [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst -# M429:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M428:xyzbc kins (motion.switchkins-type==1) -# M430:userk kins (motion.switchkins-type==2) +# M429:identity kins (kinstype 0, startupDEFAULT) +# M428:xyzbc kins (kinstype 1) +# M430:userk kins (kinstype 2) MDI_COMMAND = M429 MDI_COMMAND = M428 MDI_COMMAND = M430 diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.txt b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.txt index 4641cf6da28..20595fb39c7 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.txt +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.txt @@ -6,11 +6,9 @@ Uses remapped user m codes for kins switch: M428: XYZBC (TCP) M430: userk Kinematics -A hal net is required to connect the -analog out pin N, Example (for N=3): - - net :kinstype-select <= motion.analog-out-03 - net :kinstype-select => motion.switchkins-type +The kinematics type is selected with +G12.1 and G13.1, no hal connection is +required. Hal Input pins: xyzbc-trt-kins.x-offset diff --git a/configs/sim/axis/vismach/hexapod-sim/hexapod.ini b/configs/sim/axis/vismach/hexapod-sim/hexapod.ini index bf07a3c0cce..f42a14799a0 100644 --- a/configs/sim/axis/vismach/hexapod-sim/hexapod.ini +++ b/configs/sim/axis/vismach/hexapod-sim/hexapod.ini @@ -40,7 +40,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = kinematics.hal HALCMD = loadusr -W ./hexagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = hexapod_postgui.hal [HALUI] diff --git a/configs/sim/axis/vismach/hexapod-sim/remap_subs/428remap.ngc b/configs/sim/axis/vismach/hexapod-sim/remap_subs/428remap.ngc index 4ab3aaf922d..e9529f6d0f8 100644 --- a/configs/sim/axis/vismach/hexapod-sim/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/hexapod-sim/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype=0 genhexkins o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/hexapod-sim/remap_subs/429remap.ngc b/configs/sim/axis/vismach/hexapod-sim/remap_subs/429remap.ngc index 54726d37a6c..0291e69889d 100644 --- a/configs/sim/axis/vismach/hexapod-sim/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/hexapod-sim/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 Identity kinematics o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/hexapod-sim/remap_subs/430remap.ngc b/configs/sim/axis/vismach/hexapod-sim/remap_subs/430remap.ngc index 7586236a003..886fe727740 100644 --- a/configs/sim/axis/vismach/hexapod-sim/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/hexapod-sim/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 userk kins o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/README b/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/README index fe67738ec65..cfb0fa8ecc2 100644 --- a/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/README +++ b/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/README @@ -13,10 +13,10 @@ with 6 revolute joints. 2) pyvcp buttons are provided to switch between 'genserkins' and 'identity' kinematics. The buttons issue remapped -commands M428,M429. These commands - a) set the motion.switchkins-type pin and - b) force a synchronization using a -motion input read command. +commands M428,M429. These commands +select the kinematics with G12.1, which +synchronizes interpreter and motion +itself. 3) when set for 'identity' kins, default assignments are: diff --git a/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/melfa-sim-genser.ini b/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/melfa-sim-genser.ini index 315bae0831d..cfea9e01701 100644 --- a/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/melfa-sim-genser.ini +++ b/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/melfa-sim-genser.ini @@ -12,7 +12,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = melfa_mdh.hal HALCMD = loadusr -W ../melfagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = ../melfa-postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/README b/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/README index 0ca33f8b779..c3f60434b89 100644 --- a/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/README +++ b/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/README @@ -11,10 +11,10 @@ with 6 revolute joints. 2) pyvcp buttons are provided to switch between 'three21' and 'identity' kinematics. The buttons issue remapped -commands M428,M429. These commands - a) set the motion.switchkins-type pin and - b) force a synchronization using a -motion input read command. +commands M428,M429. These commands +select the kinematics with G12.1, which +synchronizes interpreter and motion +itself. 3) when set for 'identity' kins, default assignments are: diff --git a/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/melfa_321.ini b/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/melfa_321.ini index 0419806ad89..5a8ba9d4787 100644 --- a/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/melfa_321.ini +++ b/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/melfa_321.ini @@ -12,7 +12,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = melfa_dh.hal HALCMD = loadusr -W ../melfagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = ../melfa-postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/melfa-sim/remap_subs/428remap.ngc b/configs/sim/axis/vismach/melfa-sim/remap_subs/428remap.ngc index 8669ac0e781..c7dda9ab73d 100644 --- a/configs/sim/axis/vismach/melfa-sim/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/melfa-sim/remap_subs/428remap.ngc @@ -1,28 +1,16 @@ ;M428 by remap: select genserkins o<428remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 0 ; genserkins -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]FEATURE==8) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value + G13.1 ; back to kinstype 0, syncs interp and motion G10 L2 P7 X0 Y0 Z0 A-180 B0 C0 G59.1 - M66 E0 L0 ; force synch ; (debug, M428:genserkins) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 0]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE 0]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/melfa-sim/remap_subs/429remap.ngc b/configs/sim/axis/vismach/melfa-sim/remap_subs/429remap.ngc index 32dff4d4742..2d28bda961a 100644 --- a/configs/sim/axis/vismach/melfa-sim/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/melfa-sim/remap_subs/429remap.ngc @@ -1,28 +1,16 @@ ;M429 by remap: select identity kins o<429remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 1 ; identity kins -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]FEATURE==8) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value + G12.1 P# ; select kinstype, syncs interp and motion G10 L2 P8 X0 Y-90 Z0 A0 B90 C0 G59.2 - M66 E0 L0 ; force synch ; (debug, M429:identity kins) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 1]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE 1]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/melfa-sim/remap_subs/430remap.ngc b/configs/sim/axis/vismach/melfa-sim/remap_subs/430remap.ngc index c7d087435f6..e81d4ed4ac2 100644 --- a/configs/sim/axis/vismach/melfa-sim/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/melfa-sim/remap_subs/430remap.ngc @@ -1,26 +1,14 @@ ;M430 by remap: select gensertool kins o<430remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 2 ; gensertool kins -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M430:Missing [RS274NGC]FEATURE==8) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion ; (debug, M429:identity kins) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 2]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE 2]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/puma/puma.ini b/configs/sim/axis/vismach/puma/puma.ini index c6bbd33f245..99c0fa33112 100644 --- a/configs/sim/axis/vismach/puma/puma.ini +++ b/configs/sim/axis/vismach/puma/puma.ini @@ -12,7 +12,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = puma_dh.hal HALCMD = loadusr -W ./pumagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = puma_postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/puma/puma560.halshow b/configs/sim/axis/vismach/puma/puma560.halshow index 11b090d98c6..eb532d6e928 100644 --- a/configs/sim/axis/vismach/puma/puma560.halshow +++ b/configs/sim/axis/vismach/puma/puma560.halshow @@ -1,4 +1,4 @@ -pin+motion.switchkins-type +pin+motion.kins-type pin+kinstype.is-0 pin+kinstype.is-1 pin+kinstype.is-2 diff --git a/configs/sim/axis/vismach/puma/puma560.ini b/configs/sim/axis/vismach/puma/puma560.ini index 5e29092ba5c..046f5d118d5 100644 --- a/configs/sim/axis/vismach/puma/puma560.ini +++ b/configs/sim/axis/vismach/puma/puma560.ini @@ -16,7 +16,6 @@ HALUI = halui HALCMD = loadusr -W ./puma560gui.py HALFILE = LIB:basic_sim.tcl HALFILE = puma560_dh.hal -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = puma560_postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/puma/puma560.txt b/configs/sim/axis/vismach/puma/puma560.txt index 4fb318abbba..7353b02753c 100644 --- a/configs/sim/axis/vismach/puma/puma560.txt +++ b/configs/sim/axis/vismach/puma/puma560.txt @@ -8,10 +8,10 @@ with 6 revolute joints. 2) pyvcp buttons are provided to switch between genserkins and identity kinematics. The buttons issue remapped -commands M428,M429. These commands a) -set the motion.switchkins-type pin and -b) force a synchronization using a -motion input read command. +commands M428,M429. These commands +select the kinematics with G12.1, which +synchronizes interpreter and motion +itself. 3) when set for identity kins, default assignments are: diff --git a/configs/sim/axis/vismach/puma/puma560_uvw.ini b/configs/sim/axis/vismach/puma/puma560_uvw.ini index 97ec74cb5f6..a6b9f8544e9 100644 --- a/configs/sim/axis/vismach/puma/puma560_uvw.ini +++ b/configs/sim/axis/vismach/puma/puma560_uvw.ini @@ -16,7 +16,6 @@ HALUI = halui HALCMD = loadusr -W ./puma560gui.py HALFILE = LIB:basic_sim.tcl HALFILE = puma560_dh.hal -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = puma560_postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/puma/puma_cube.ini b/configs/sim/axis/vismach/puma/puma_cube.ini index 7cf35ce23e3..8397ce3ea7a 100644 --- a/configs/sim/axis/vismach/puma/puma_cube.ini +++ b/configs/sim/axis/vismach/puma/puma_cube.ini @@ -103,7 +103,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = puma_dh.hal HALCMD = loadusr -W ./pumagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = puma_postgui.hal [HALUI] diff --git a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc index 36f8ee3e499..2b2016bfe50 100644 --- a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype=0 (default) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/puma/remap_subs/429remap.ngc b/configs/sim/axis/vismach/puma/remap_subs/429remap.ngc index 627d547052b..25a2ef41339 100644 --- a/configs/sim/axis/vismach/puma/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/puma/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 (Identity kinematics) o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/puma/remap_subs/430remap.ngc b/configs/sim/axis/vismach/puma/remap_subs/430remap.ngc index 5af12f4fbf2..f5d2db707aa 100644 --- a/configs/sim/axis/vismach/puma/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/puma/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 (userk kins) o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/scara/remap_subs/428remap.ngc b/configs/sim/axis/vismach/scara/remap_subs/428remap.ngc index 8698782fff7..f983c3870ea 100644 --- a/configs/sim/axis/vismach/scara/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/scara/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype==0 (default) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/scara/remap_subs/429remap.ngc b/configs/sim/axis/vismach/scara/remap_subs/429remap.ngc index 627d547052b..25a2ef41339 100644 --- a/configs/sim/axis/vismach/scara/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/scara/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 (Identity kinematics) o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/scara/remap_subs/430remap.ngc b/configs/sim/axis/vismach/scara/remap_subs/430remap.ngc index 5af12f4fbf2..f5d2db707aa 100644 --- a/configs/sim/axis/vismach/scara/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/scara/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 (userk kins) o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/scara/scara.ini b/configs/sim/axis/vismach/scara/scara.ini index 6b14a4eaeb3..23323fdc8bf 100644 --- a/configs/sim/axis/vismach/scara/scara.ini +++ b/configs/sim/axis/vismach/scara/scara.ini @@ -58,7 +58,6 @@ KINEMATICS = scarakins coordinates=xyzcab HALUI = halui HALFILE = LIB:basic_sim.tcl HALCMD = loadusr -W ./scaragui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = scara_postgui.hal [HALUI] diff --git a/configs/sim/qtaxis/non-trivial/scara/remap_subs/428remap.ngc b/configs/sim/qtaxis/non-trivial/scara/remap_subs/428remap.ngc index 8698782fff7..f983c3870ea 100644 --- a/configs/sim/qtaxis/non-trivial/scara/remap_subs/428remap.ngc +++ b/configs/sim/qtaxis/non-trivial/scara/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype==0 (default) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/qtaxis/non-trivial/scara/remap_subs/429remap.ngc b/configs/sim/qtaxis/non-trivial/scara/remap_subs/429remap.ngc index 627d547052b..25a2ef41339 100644 --- a/configs/sim/qtaxis/non-trivial/scara/remap_subs/429remap.ngc +++ b/configs/sim/qtaxis/non-trivial/scara/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 (Identity kinematics) o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/qtaxis/non-trivial/scara/remap_subs/430remap.ngc b/configs/sim/qtaxis/non-trivial/scara/remap_subs/430remap.ngc index 5af12f4fbf2..f5d2db707aa 100644 --- a/configs/sim/qtaxis/non-trivial/scara/remap_subs/430remap.ngc +++ b/configs/sim/qtaxis/non-trivial/scara/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 (userk kins) o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/428remap.ngc b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/428remap.ngc index 8698782fff7..f983c3870ea 100644 --- a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/428remap.ngc +++ b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype==0 (default) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/429remap.ngc b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/429remap.ngc index 627d547052b..25a2ef41339 100644 --- a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/429remap.ngc +++ b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 (Identity kinematics) o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/430remap.ngc b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/430remap.ngc index 5af12f4fbf2..f5d2db707aa 100644 --- a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/430remap.ngc +++ b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 (userk kins) o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub From f9efeb3243fb6223247881086831794f4c095c2b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 19:13:27 +1000 Subject: [PATCH 08/20] docs: stop offering the deprecated pin as an equal way to switch The G-code chapter told the reader a config may select the kinematics "from G-code, from that pin, or from both", and the switchkins chapter said the same twice, in its introduction and again under G-code commands. All three predate motion reporting the pin as deprecated, and they contradict it. They now say the pin is deprecated and why, in the same words as the man page. The G-code chapter keeps the fact that the pin takes the same numbering, which is what somebody migrating away from it needs to know. --- docs/src/gcode/g-code.adoc | 11 +++++++---- docs/src/motion/switchkins.adoc | 24 +++++++++++++----------- 2 files changed, 20 insertions(+), 15 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index e173c9b54c7..befff8df401 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -945,10 +945,13 @@ G13.1 'G12.1' selects one of the kinematics provided by a switchable kinematics module, and 'G13.1' cancels back to kinematics 0. The 'P' word is the -kinematics number, the same number that the `motion.switchkins-type` pin -takes, so 'G13.1' and `G12.1 P0` do the same thing. A config may select -the kinematics from G-code, from that pin, or from both: each is acted on -when it changes, so the most recent request is the one in force. +kinematics number, so 'G13.1' and `G12.1 P0` do the same thing. + +These are the way to select a kinematics. The `motion.switchkins-type` +HAL pin does the same thing and takes the same numbering, but it is +deprecated: the interpreter never sees it, so a program is read, its +limits checked and its path looked ahead in whatever kinematics the +interpreter last knew about, which need not be the one that runs it. Both codes are queue synchronisation points. The interpreter waits for queued motion to finish before the kinematics changes, so no move is ever diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 67eab672180..dbda6e4cc52 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -21,17 +21,18 @@ switched to identity kinematics for control of individual joints after homing. The kinematics type is selected with 'G12.1 P-' and 'G13.1', from a -G-code program or by interactive MDI commands. It can also be selected -by a motion module HAL pin, which allows the halui provisions for -activating MDI commands to be used so that buttons select the -kinematics type from hardware controls or a virtual panel (PyVCP, -GladeVCP, etc.). +G-code program or by interactive MDI commands. Buttons on a virtual +panel (PyVCP, GladeVCP, etc.) or on hardware controls select a +kinematics type through the halui provisions for activating MDI +commands. Changing the kinematics type requires the interpreter and motion parts -of LinuxCNC to be *synchronized*. 'G12.1' and 'G13.1' do this -themselves. When the HAL pin is written instead, the G-code must force -synchronization, typically with a HAL pin 'read' command (M66 E0 L0) -immediately after altering the pin. +of LinuxCNC to be *synchronized*, which 'G12.1' and 'G13.1' do +themselves. + +A deprecated HAL pin, 'motion.switchkins-type', selects a kinematics +type as well. It is described under Usage below, because existing +configurations use it. == Switchable Kinematic Modules @@ -178,8 +179,9 @@ These codes ask motion for the kinstype directly and synchronize task and motion themselves, so no HAL connection and no separate sync command are needed. The G-code words and the *motion.switchkins-type* pin are both acted on when they change, so whichever asked most recently is the one in -force, and a config can use either or both. *motion.kins-type* reports -what is currently selected. +force. *motion.kins-type* reports what is currently selected. + +The pin is deprecated, see the warning under HAL Connections. The kinstype in force is readable in G-code as '#<_kins_type>', which lets a subroutine restore whatever its caller had selected: From dcadfa1adcf3cb103df28a56fe914d1f0333612f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 21:02:46 +1000 Subject: [PATCH 09/20] docs: lead with the deprecation notice for the switchkins pin The paragraph read as though the pin were an equal alternative that happened to carry a caveat. State the deprecation first, as a warning. --- docs/src/gcode/g-code.adoc | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index befff8df401..487add73f74 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -947,11 +947,13 @@ G13.1 module, and 'G13.1' cancels back to kinematics 0. The 'P' word is the kinematics number, so 'G13.1' and `G12.1 P0` do the same thing. -These are the way to select a kinematics. The `motion.switchkins-type` -HAL pin does the same thing and takes the same numbering, but it is -deprecated: the interpreter never sees it, so a program is read, its -limits checked and its path looked ahead in whatever kinematics the -interpreter last knew about, which need not be the one that runs it. +[WARNING] +Deprecation notice: selecting the kinematics by writing the +`motion.switchkins-type` HAL pin is deprecated. It takes the same +numbering and still works, but it does not tell the interpreter that +anything changed, so a program is read, its limits checked and its path +looked ahead in whatever kinematics the interpreter last knew about, +which need not be the one that runs it. Use 'G12.1' and 'G13.1'. Both codes are queue synchronisation points. The interpreter waits for queued motion to finish before the kinematics changes, so no move is ever From 82ca43eb77f81a86e026b0282853e1145c5b8042 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 11 Aug 2026 22:59:37 +1000 Subject: [PATCH 10/20] switchkins: separate the dispatch from rtapi_app_main() switchkins.c owned rtapi_app_main(), so a module could only use it by having no main of its own. That ruled out halcompile components, which is why the switchable kinematics in hal/components each carry a private copy of the dispatch, the kinstype pins and the switch statement. Move rtapi_app_main(), rtapi_app_exit() and the coordinates= and sparm= module parameters to switchkins_main.c, and give switchkins.c a single entry point: int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates); It counts and validates the registered types, creates the pins and starts on type 0. The caller owns the hal component, doing hal_init() before and hal_ready() after, so anything that already has a component can use switchkins by calling this. The types switchkinsSetup() supplies now reach the arrays through switchkinsRegister() like any others, rather than being written directly through its out parameters. One registration path means the checks apply to every type, so a module that both fills an argument and registers the same type is refused rather than silently overwriting. The eight existing modules gain switchkins_main.o in their -objs and are otherwise untouched. --- src/Makefile | 8 +++ src/emc/kinematics/switchkins.c | 62 ++++++------------ src/emc/kinematics/switchkins.h | 11 +++- src/emc/kinematics/switchkins_main.c | 94 ++++++++++++++++++++++++++++ 4 files changed, 130 insertions(+), 45 deletions(-) create mode 100644 src/emc/kinematics/switchkins_main.c diff --git a/src/Makefile b/src/Makefile index 2c56a18ed3a..9f33ba7a0d4 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1190,6 +1190,7 @@ genhexkins-objs += libposemath/_posemath.o genhexkins-objs += $(MATHSTUB) genhexkins-objs += emc/kinematics/kins_util.o genhexkins-objs += emc/kinematics/switchkins.o +genhexkins-objs += emc/kinematics/switchkins_main.o genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o @@ -1199,6 +1200,7 @@ genserkins-objs += libposemath/gomath.o genserkins-objs += $(MATHSTUB) genserkins-objs += emc/kinematics/kins_util.o genserkins-objs += emc/kinematics/switchkins.o +genserkins-objs += emc/kinematics/switchkins_main.o genserkins-objs += $(USERKFUNCS) obj-m += xyzac-trt-kins.o @@ -1206,6 +1208,7 @@ xyzac-trt-kins-objs := emc/kinematics/xyzac-trt-kins.o xyzac-trt-kins-objs += emc/kinematics/trtfuncs.o xyzac-trt-kins-objs += emc/kinematics/kins_util.o xyzac-trt-kins-objs += emc/kinematics/switchkins.o +xyzac-trt-kins-objs += emc/kinematics/switchkins_main.o xyzac-trt-kins-objs += $(USERKFUNCS) obj-m += xyzbc-trt-kins.o @@ -1213,6 +1216,7 @@ xyzbc-trt-kins-objs := emc/kinematics/xyzbc-trt-kins.o xyzbc-trt-kins-objs += emc/kinematics/trtfuncs.o xyzbc-trt-kins-objs += emc/kinematics/kins_util.o xyzbc-trt-kins-objs += emc/kinematics/switchkins.o +xyzbc-trt-kins-objs += emc/kinematics/switchkins_main.o xyzbc-trt-kins-objs += $(USERKFUNCS) obj-m += scarakins.o @@ -1221,6 +1225,7 @@ scarakins-objs += libposemath/_posemath.o scarakins-objs += $(MATHSTUB) scarakins-objs += emc/kinematics/kins_util.o scarakins-objs += emc/kinematics/switchkins.o +scarakins-objs += emc/kinematics/switchkins_main.o scarakins-objs += $(USERKFUNCS) obj-m += pumakins.o @@ -1229,6 +1234,7 @@ pumakins-objs += libposemath/_posemath.o pumakins-objs += $(MATHSTUB) pumakins-objs += emc/kinematics/kins_util.o pumakins-objs += emc/kinematics/switchkins.o +pumakins-objs += emc/kinematics/switchkins_main.o pumakins-objs += $(USERKFUNCS) obj-m += three21kins.o @@ -1237,6 +1243,7 @@ three21kins-objs += libposemath/_posemath.o three21kins-objs += $(MATHSTUB) three21kins-objs += emc/kinematics/kins_util.o three21kins-objs += emc/kinematics/switchkins.o +three21kins-objs += emc/kinematics/switchkins_main.o three21kins-objs += $(USERKFUNCS) obj-m += 5axiskins.o @@ -1245,6 +1252,7 @@ obj-m += 5axiskins.o 5axiskins-objs += $(MATHSTUB) 5axiskins-objs += emc/kinematics/kins_util.o 5axiskins-objs += emc/kinematics/switchkins.o +5axiskins-objs += emc/kinematics/switchkins_main.o 5axiskins-objs += $(USERKFUNCS) #---------------------------------------------------------------- diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index f1393867e35..68cf8e36e25 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,7 +27,6 @@ * Using modules must supply function: switchkinsSetup() */ #include -#include #include #include #include @@ -240,47 +239,31 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) return 0; } // switchkinsRegister() -//********************************************************************* -static char *coordinates; -RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static char *sparm; -RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); - EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(switchkinsRegister); -MODULE_LICENSE("GPL"); +EXPORT_SYMBOL(switchkinsInit); -static int comp_id; //********************************************************************* -int rtapi_app_main(void) +// The caller owns the hal component: it does hal_init() before this and +// hal_ready() after it. Every switchkins-type must be registered by +// now. +int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates) { - int i,res; - char* emsg="other"; - - // defaults prior to switchkinsSetup() call - kp.kinsname = NULL; - kp.halprefix = NULL; - kp.required_coordinates = ""; - kp.max_joints = 0; // Setup must supply - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; // negative means: not used - - kp.sparm = sparm; // module parm passed to kins - - // may also call switchkinsRegister() - res = switchkinsSetup(&kp, - &ksetups[0], &ksetups[1], &ksetups[2], - &kfwds[0], &kfwds[1], &kfwds[2], - &kinvs[0], &kinvs[1], &kinvs[2]); - if (res) {emsg="switchkinsSetp FAIL"; goto error;} - if (register_error) {emsg="switchkinsRegister FAIL"; goto error;} - - // the highest type provided by either route sets the count + int i; + int res = 0; + char* emsg = "other"; + + kp = *ksetup_parms; // kinematics parms are needed after this returns + + if (register_error) {emsg = "switchkinsRegister FAIL"; goto error;} + + // the highest type registered sets the count for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } } @@ -319,11 +302,8 @@ int rtapi_app_main(void) emsg = "incomplete switchkins-type"; goto error; } - comp_id = hal_init(kp.kinsname); - if(comp_id < 0) goto error; - swdata = hal_malloc(sizeof(struct swdata)); - if (!swdata) goto error; + if (!swdata) {emsg = "hal_malloc fail"; goto error;} for (i=0; i < kins_count; i++) { res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is[i]), @@ -337,8 +317,8 @@ int rtapi_app_main(void) res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_a, 0.0, "skgui.a"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_b, 0.0, "skgui.b"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_c, 0.0, "skgui.c"); - if (res) {emsg = "hal pin create fail";goto error;} } + if (res) {emsg = "hal pin create fail"; goto error;} switchkins_type = 0; // startup with default type kinematicsSwitch(switchkins_type); @@ -349,14 +329,10 @@ int rtapi_app_main(void) ksetups[i](comp_id,coordinates,&kp); } - hal_ready(comp_id); return 0; error: rtapi_print_msg(RTAPI_MSG_ERR, "\nSwitchkins FAIL %s:<%s>\n",kp.kinsname,emsg); - hal_exit(comp_id); return -1; -} // rtapi_app_main() - -void rtapi_app_exit(void) { hal_exit(comp_id); } +} // switchkinsInit() diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 2f9ee530a7c..80c02613bb1 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -28,13 +28,20 @@ typedef int (*KS)(const int comp_id, // halpins ); //********************************************************************* -// supplied by the using module, provides types 0,1,2 +// supplied by a module using switchkins_main.c, provides types 0,1,2 extern int switchkinsSetup(kparms* ksetup_parms, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, KI* kinv0, KI* kinv1, KI* kinv2 ); -// called from switchkinsSetup(), once per type it does not provide itself +// provide one switchkins-type, before switchkinsInit() extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); + +// create the hal pins and start on type 0; the caller owns the hal +// component and does hal_init() before and hal_ready() after +extern int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates + ); #endif // } diff --git a/src/emc/kinematics/switchkins_main.c b/src/emc/kinematics/switchkins_main.c new file mode 100644 index 00000000000..4a4cc05153c --- /dev/null +++ b/src/emc/kinematics/switchkins_main.c @@ -0,0 +1,94 @@ +/* + Copyright 2019 Dewey Garrett + + 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. +*/ + +/* switchkins_main.c provides rtapi_app_main() for kinematics modules +* built around switchkins.c. A module that gets its rtapi_app_main() +* from somewhere else (a halcompile component, for instance) links +* switchkins.c alone and calls switchkinsInit() itself. +* +* Using modules must supply function: switchkinsSetup() +*/ +#include +#include +#include + +#include "switchkins.h" + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); +static char *sparm; +RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); + +MODULE_LICENSE("GPL"); + +static int comp_id = -1; + +int rtapi_app_main(void) +{ + kparms kp; + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + // defaults prior to switchkinsSetup() call + kp.kinsname = NULL; + kp.halprefix = NULL; + kp.required_coordinates = ""; + kp.max_joints = 0; // Setup must supply + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; // negative means: not used + + kp.sparm = sparm; // module parm passed to kins + + // switchkinsSetup() provides types 0,1,2 and may also call + // switchkinsRegister() for any others + if (switchkinsSetup(&kp, + &ksetup[0], &ksetup[1], &ksetup[2], + &kfwd[0], &kfwd[1], &kfwd[2], + &kinv[0], &kinv[1], &kinv[2])) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + // the types switchkinsSetup() supplied go in by the same route as + // any other, so that providing one twice is caught + for (i=0; i < 3; i++) { + if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } + if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } + } + + if (!kp.kinsname) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + comp_id = hal_init(kp.kinsname); + if (comp_id < 0) return comp_id; + + if (switchkinsInit(comp_id, &kp, coordinates)) { + hal_exit(comp_id); + return -1; + } + + hal_ready(comp_id); + return 0; +} // rtapi_app_main() + +void rtapi_app_exit(void) { hal_exit(comp_id); } From bc6e7f683f0d917f8f0305bdd7cee6b1a018b4a0 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 00:17:36 +1000 Subject: [PATCH 11/20] switchkins: let halcompile components use the switchkins core millturn, xyzab_tdr_kins, xyzacb_trsrn and xyzbca_trsrn each carried their own copy of the switchkins dispatch: a private switchkins_type, a kinematicsSwitch() with a hand-written case per type, and a setup routine that had to hal_set_unready() the component again because it ran from kinematicsType(), long after halcompile had called hal_ready(). Four copies of the same thing, none of them sharing the fixes made to switchkins.c. They could not link switchkins.o before, because switchkins.c supplied rtapi_app_main() and so does halcompile. Now that the dispatch is separate from the 'main' program, a component can link it and call switchkinsInit() from EXTRA_SETUP(), which halcompile runs after hal_init() and before hal_ready(). Two build changes make that possible: - the generated per-comp .mak takes a -extra-objs list, so a .comp can name objects besides its own. - switchkins.h is copied to ../include and installed, so resolves from a generated component source. Each of the four now registers its kinematics types and calls switchkinsInit(). Their identity type comes from kins_util.c, which gets them the coordinates= module parameter they never had, and a bad motion.switchkins-type is now rejected and leaves the running kinematics alone instead of stranding the module on a type that does not exist. Pin names are unchanged, except that millturn's in/out example pins are gone: they were template scaffolding copied from userkins.comp, unused by the sim config, and a kinematics-type setup routine is where kinematics pins belong now. millturn keeps its fpin pin and fdemo function. The xyzab-tdr, xyzacb-trsrn, xyzbca-trsrn and millturn sim configs give the same positions through the same MDI sequence as before, to four decimals, in every kinematics type. --- docs/src/motion/switchkins.adoc | 86 ++++-- share/linuxcnc/kins_util.c | 366 +++++++++++++++++++++++++ share/linuxcnc/switchkins.c | 337 +++++++++++++++++++++++ src/Makefile | 1 + src/emc/kinematics/switchkins.h | 8 +- src/hal/components/Submakefile | 13 +- src/hal/components/millturn.comp | 220 +++++---------- src/hal/components/xyzab_tdr_kins.comp | 293 ++++++++------------ src/hal/components/xyzacb_trsrn.comp | 276 ++++++++----------- src/hal/components/xyzbca_trsrn.comp | 276 ++++++++----------- 10 files changed, 1196 insertions(+), 680 deletions(-) create mode 100644 share/linuxcnc/kins_util.c create mode 100644 share/linuxcnc/switchkins.c diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index dbda6e4cc52..18f1eab0d36 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -46,6 +46,10 @@ The following kinematics modules support switchable kinematics: . *three21kins* (type0:three21kins type1:identity) . *scarakins* (type0:scarakins type1:identity) . *5axiskins* (type0:5axiskins type1:identity) (bridgemill) +. *millturn* (type0:identity type1:turn) +. *xyzab_tdr_kins* (type0:identity type1:tcp) +. *xyzacb_trsrn* (type0:identity type1:tcp type2:tool) +. *xyzbca_trsrn* (type0:identity type1:tcp type2:tool) The xyz[ab]c-trt-kins modules by default use type0==xyz[ab]c-trt-kins for backwards compatibility. The provided sim configs alter the @@ -395,6 +399,10 @@ configs/sim/axis/vismach/ . . puma/puma560.ini (genserkins) . puma/puma.ini (pumakins) . hexapod-sim/hexapod.ini (genhexkins) +. millturn/millturn.ini (millturn) +. 5axis/table-dual-rotary/xyzab-tdr.ini (xyzab_tdr_kins) +. 5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini (xyzacb_trsrn) +. 5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini (xyzbca_trsrn) == User kinematics provisions @@ -442,19 +450,14 @@ protocols. == Code Notes Kinematic modules providing switchkins functionality are linked to -the switchkins.o object (switchkins.c) that provides the module -'main' program (rtapi_app_main()) and related functions. This -'main' program reads (optional) module command-line parameters -(coordinates, sparm) and passes them to the module-provided -function switchkinsSetup(). - -The switchkinsSetup() function identifies kinstype-specific setup -routines and the functions for forward an inverse calculation for -each kinstype (0,1,2) and sets a number of configuration -settings. - -A module can provide further kinstypes by calling -switchkinsRegister() from within switchkinsSetup(), once per +the switchkins.o object (switchkins.c). It provides +kinematicsForward(), kinematicsInverse(), kinematicsSwitch() and +the rest of the kinematics interface, dispatching each call to the +kinstype currently selected, and it creates the HAL pins common to +all switchkins modules. It does not provide the module 'main' +program, so a module can get that from wherever suits it. + +A kinstype is supplied by calling switchkinsRegister(), once per kinstype: ---- @@ -462,25 +465,60 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- 'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in -switchkins.h). A kinstype has to come from one route or the -other, so registering one that switchkinsSetup() has already -filled in is an error, and so is leaving a gap below the highest -kinstype provided. Either mistake fails the module load and says -which kinstype is at fault. +switchkins.h). Registering a kinstype twice is an error, and so is +leaving a gap below the highest kinstype provided. Either mistake +fails the module load and says which kinstype is at fault. Each kinstype gets its own 'kinstype.is-N' pin, so a module providing the usual three keeps the pin names it always had. -After calling switchkinsSetup(), rtapi_app_main() checks the -supplied parameters, creates a HAL component, and then invokes -the setup routine identified for each kinstype. +When every kinstype is registered, the module calls: + +---- +int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates); +---- + +which checks the supplied parameters, creates the HAL pins, selects +kinstype 0, and then invokes the setup routine registered for each +kinstype. The caller owns the HAL component: it does hal_init() +before switchkinsInit() and hal_ready() after it. Each kinstype setup routine can (optionally) create HAL pins and set them to default values. A setup routine is called once per kinstype it is registered for, so a routine used for two -kinstypes must not create the same pin twice. When all setup -routines finish, rtapi_app_main() issues hal_ready() for the -component to complete creation of the module. +kinstypes must not create the same pin twice. + +=== Module main program + +A module written as a plain C file links switchkins_main.o +(switchkins_main.c) for its rtapi_app_main(). That 'main' program +reads the (optional) module command-line parameters (coordinates, +sparm) and passes them to the module-provided function +switchkinsSetup(): + +---- +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2); +---- + +which identifies the setup, forward and inverse routines for +kinstypes 0,1,2 and sets a number of configuration settings. Those +three are registered for the module, so it can supply further +kinstypes by calling switchkinsRegister() itself, and registering +one that switchkinsSetup() has already filled in is the same error +as any other duplicate. + +A module written as a halcompile component gets rtapi_app_main() +from halcompile instead. It registers its kinstypes and calls +switchkinsInit() from its EXTRA_SETUP() routine, which halcompile +runs after hal_init() and before hal_ready(). The component names +the objects it needs in hal/components/Submakefile: + +---- +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +---- === Outline diff --git a/share/linuxcnc/kins_util.c b/share/linuxcnc/kins_util.c new file mode 100644 index 00000000000..c82a4a2fc95 --- /dev/null +++ b/share/linuxcnc/kins_util.c @@ -0,0 +1,366 @@ +/* Utility routines for kinematics modules +** License GPL Version 2 +** +** utilities for use with switchkins.c +**--------------------------------------------------------------------- +** identityKinematicsSetup() +** identityKinematicsForward() +** identityKinematicsInverse() +** +** Routines for identity kinematics using mapping created by +** map_coordinates_to_jnumbers() +** +**--------------------------------------------------------------------- +** map_coordinates_to_jnumbers() +** +** Map a string of coordinate letters to joint numbers sequentially. +** If allow_duplicates==1, a coordinate letter may be specified more +** than once to assign it to multiple joint numbers (the kinematics +** module must support such usage). +** +** Default mapping if coordinates==NULL is: +** X:0 Y:1 Z:2 A:3 B:4 C:5 U:6 V:7 W:8 +** +** Example coordinates-to-joints mappings: +** coordinates=XYZ X:0 Y:1 Z:2 +** coordinates=ZYX Z:0 Y:1 X:2 +** coordinates=XYZZZZ x:0 Y:1 Z:2,3,4,5 +** coordinates=XXYZ X:0,1 Y:2 Z:3 +**--------------------------------------------------------------------- +** +** mapped_joints_to_position() +** +** Update position based mapping created by map_coordinates_to_jnumbers() +** (used for identity-based forward kinematics) +**--------------------------------------------------------------------- +** +** position_to_mapped_joints() +** +** Update joints (including joints for duplicate letters) +** based on mapping created by map_coordinates_to_jnumbers() +** (used for identity-based inverse kinematics) +** +**--------------------------------------------------------------------- +*/ + +#include +#include +#include +#include +#include + +// principal joint numbers based on module 'coordinates' parameter +static int JX = -1; +static int JY = -1; +static int JZ = -1; +static int JA = -1; +static int JB = -1; +static int JC = -1; +static int JU = -1; +static int JV = -1; +static int JW = -1; + +// bitmaps indicate joints used for each axis letter +static int X_joints_bitmap; +static int Y_joints_bitmap; +static int Z_joints_bitmap; +static int A_joints_bitmap; +static int B_joints_bitmap; +static int C_joints_bitmap; +static int U_joints_bitmap; +static int V_joints_bitmap; +static int W_joints_bitmap; + +static int map_initialized = 0; +#define MAX_COORDINATES_CHARS 32 +static char used_coordinates[MAX_COORDINATES_CHARS+1]; + +int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[] ) //result +{ + char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; + int jno=0; + bool found=0; + int dups[EMCMOT_MAX_AXIS]; + const char *coords = coordinates; + char coord_letter[] = {'X','Y','Z','A','B','C','U','V','W'}; + int i; + + if (strlen(coordinates) > MAX_COORDINATES_CHARS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers too many chars:%s\n" + ,__FILE__,coordinates); + return -1; + + } + // Note: may be called multiple times for different switchkins + // types but coordinates must agree + if (used_coordinates[0] == 0) { + strcpy(used_coordinates,coordinates); + } else { + if (strcasecmp(coordinates,used_coordinates)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers altered:%s %s\n" + ,__FILE__,used_coordinates,coordinates); + return -1; + } + } + for (i=0; i EMCMOT_MAX_JOINTS) ) { + rtapi_print_msg(RTAPI_MSG_ERR,"%s bogus max_joints=%d\n", + errtag,max_joints); + return -1; + } + + // init all axis_idx_for_jno[] (-1 means unspecified) + for(jno=0; jno max_joints) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s too many coordinates <%s> for max_joints=%d\n", + errtag,coordinates,max_joints); + return -1; + } + } // while + + if (!found) { + rtapi_print_msg(RTAPI_MSG_ERR,"%s missing coordinates '%s'\n", + errtag,coordinates); + return -1; + } + if (!allow_duplicates) { + int ano; + for(ano=0; ano 1) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s duplicates not allowed in coordinates=%s, letter=%c\n", + errtag,coordinates,coord_letter[ano]); + return -1; + } + } + } + + for (jno=0; jno < max_joints; jno++) { + int bitnumber = 1<tran.x = joints[JX]; + if ( bit & Y_joints_bitmap ) pos->tran.y = joints[JY]; + if ( bit & Z_joints_bitmap ) pos->tran.z = joints[JZ]; + if ( bit & A_joints_bitmap ) pos->a = joints[JA]; + if ( bit & B_joints_bitmap ) pos->b = joints[JB]; + if ( bit & C_joints_bitmap ) pos->c = joints[JC]; + if ( bit & U_joints_bitmap ) pos->u = joints[JU]; + if ( bit & V_joints_bitmap ) pos->v = joints[JV]; + if ( bit & W_joints_bitmap ) pos->w = joints[JW]; + } + return 0; +} // mapped_joints_to_position() + +int position_to_mapped_joints(const int max_joints, + const EmcPose * pos, + double* joints) +{ + int jno; + if (!map_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "position_to_mapped_joints before map_initialized\n"); + return -1; + } + for (jno=0; jno < max_joints; jno++) { + int bit = 1<tran.x; + if ( bit & Y_joints_bitmap ) joints[jno] = pos->tran.y; + if ( bit & Z_joints_bitmap ) joints[jno] = pos->tran.z; + if ( bit & A_joints_bitmap ) joints[jno] = pos->a; + if ( bit & B_joints_bitmap ) joints[jno] = pos->b; + if ( bit & C_joints_bitmap ) joints[jno] = pos->c; + if ( bit & U_joints_bitmap ) joints[jno] = pos->u; + if ( bit & V_joints_bitmap ) joints[jno] = pos->v; + if ( bit & W_joints_bitmap ) joints[jno] = pos->w; + } + return 0; +} // position_to_mapped_joints() + +static int identity_kinematics_initialized = 0; +static int identity_max_joints; + +int identityKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + (void)comp_id; + int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; + int jno; + int show=0; + bool islathe; + + identity_max_joints = strlen(coordinates); + + if (map_coordinates_to_jnumbers(coordinates, + kp->max_joints, + kp->allow_duplicates, + axis_idx_for_jno)) { + return -1; //mapping failed + } + + /* print message for unconventional ordering; + ** a) duplicate coordinate letters + ** b) letters not ordered by "XYZABCUVW" sequence + ** (use kinstype=both works best for these) + */ + for (jno=0; jno Axis %c\n", + jno,*(p+axis_idx_for_jno[jno])); + } + if (kinematicsType() != KINEMATICS_BOTH) { + rtapi_print("identityKinematicsSetup: Recommend: kinstype=both\n"); + } + rtapi_print("\n"); + } + + identity_kinematics_initialized = 1; + return 0; +} // identityKinematicsSetup() + +int identityKinematicsForward(const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + if (!identity_kinematics_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "identityKinematicsForward: not initialized\n"); + return -1; + } + + // support multiple-joint-per-coordinate-letter assignments: + mapped_joints_to_position(identity_max_joints,joints,pos); + return 0; +} // identityKinematicsForward() + +int identityKinematicsInverse(const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + if (!identity_kinematics_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "identityKinematicsInverse: not initialized\n"); + return -1; + } + + // support multiple-joint-per-coordinate-letter assignments: + position_to_mapped_joints(identity_max_joints,pos,joints); + + return 0; +} // identityKinematicsInverse() diff --git a/share/linuxcnc/switchkins.c b/share/linuxcnc/switchkins.c new file mode 100644 index 00000000000..7a3f3d35071 --- /dev/null +++ b/share/linuxcnc/switchkins.c @@ -0,0 +1,337 @@ +/* + Copyright 2019 Dewey Garrett + + 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. +*/ + +/* switchkins.c provide functions for switchable kins modules: +* rtapi_app() +* rtapi_exit() +* kinematicsType() +* kinematicsForward() +* kinematicsInverse() +* kinematicsSwitch() +* kinematicsSwitchable() +* Using modules must supply function: switchkinsSetup() +*/ +#include +#include +#include + +#include "switchkins.h" + +//********************************************************************* +// kinematic functions (default=0 for err detection): +static kparms kp; // kinematics parms (common all types) + +// indexed by switchkins_type (NULL==not provided, for err detection): +static KS ksetups[SWITCHKINS_MAX_TYPES] = {NULL}; +static KF kfwds[SWITCHKINS_MAX_TYPES] = {NULL}; +static KI kinvs[SWITCHKINS_MAX_TYPES] = {NULL}; + +// types provided, counted in rtapi_app_main() once they are all in +static int kins_count; +static int register_error; + +static int switchkins_type; +static struct swdata { + hal_bool_t kinstype_is[SWITCHKINS_MAX_TYPES]; + + hal_real_t gui_x; + hal_real_t gui_y; + hal_real_t gui_z; + hal_real_t gui_a; + hal_real_t gui_b; + hal_real_t gui_c; +} *swdata; + +// Note: parallel kinematics (like genhexkins) often +// use iterative method for Forward algorithm +// and require an initial EmcPose. +// If fwd_iterates_mask is set +// then save/use the lastpose +static int fwd_iterates[SWITCHKINS_MAX_TYPES] = {0}; +static bool use_lastpose[SWITCHKINS_MAX_TYPES] = {0}; +static EmcPose lastpose[SWITCHKINS_MAX_TYPES]; + +static void save_lastpose(int ktype, EmcPose* pos) +{ + lastpose[ktype].tran.x = pos->tran.x; + lastpose[ktype].tran.y = pos->tran.y; + lastpose[ktype].tran.z = pos->tran.z; + lastpose[ktype].a = pos->a; + lastpose[ktype].b = pos->b; + lastpose[ktype].c = pos->c; + lastpose[ktype].u = pos->u; + lastpose[ktype].v = pos->v; + lastpose[ktype].w = pos->w; +} // save_lastpose() + +static void get_lastpose(int ktype, EmcPose* pos) +{ + pos->tran.x = lastpose[ktype].tran.x; + pos->tran.y = lastpose[ktype].tran.y; + pos->tran.z = lastpose[ktype].tran.z; + pos->a = lastpose[ktype].a; + pos->b = lastpose[ktype].b; + pos->c = lastpose[ktype].c; + pos->u = lastpose[ktype].u; + pos->v = lastpose[ktype].v; + pos->w = lastpose[ktype].w; +} // get_lastpose() + +static int gui_forward_kins(const double *joints) +{ + // the hexapod vismach gui uses these hal pins to + // display platform position/orientation in both + // genhexkins and identity kinematic types + // (similar needs for many parallel kinemtic machines) + int res; + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags; + if ( kp.gui_kinstype < 0 + || kp.gui_kinstype >= kins_count + || !kfwds[kp.gui_kinstype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "gui_forward_kins BAD gui_kinstype <%d>\n", + kp.gui_kinstype); + return -1; + } + res = kfwds[kp.gui_kinstype](joints, &lastpose[kp.gui_kinstype], + &fflags, &iflags); + hal_set_real(swdata->gui_x, lastpose[kp.gui_kinstype].tran.x); + hal_set_real(swdata->gui_y, lastpose[kp.gui_kinstype].tran.y); + hal_set_real(swdata->gui_z, lastpose[kp.gui_kinstype].tran.z); + hal_set_real(swdata->gui_a, lastpose[kp.gui_kinstype].a); + hal_set_real(swdata->gui_b, lastpose[kp.gui_kinstype].b); + hal_set_real(swdata->gui_c, lastpose[kp.gui_kinstype].c); + return res; +} // gui_forward_kins + +//********************************************************************* +int kinematicsSwitchable() {return 1;} + +int kinematicsSwitch(int new_switchkins_type) +{ + int k; + + // reject first, so a bad request leaves the running kinematics alone + if (new_switchkins_type < 0 || new_switchkins_type >= kins_count) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinematicsSwitch:BAD VALUE <%d>\n", + new_switchkins_type); + return -1; // FAIL + } + + for (k=0; k< SWITCHKINS_MAX_TYPES; k++) { use_lastpose[k] = 0;} + + switchkins_type = new_switchkins_type; + + rtapi_print_msg(RTAPI_MSG_INFO, + "kinematicsSwitch:TYPE%d\n", switchkins_type); + for (k=0; k < kins_count; k++) { + hal_set_bool(swdata->kinstype_is[k], k == switchkins_type); + } + + if (fwd_iterates[switchkins_type]) { + use_lastpose[switchkins_type] = 1; // restarting a kins types + } + return 0; // 0==> no error +} // kinematicsSwitch() + +int kinematicsForward(const double *joint, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r; + + if (fwd_iterates[switchkins_type] && use_lastpose[switchkins_type]) { + // initialize iterative forward kins (ok for identity too) + get_lastpose(switchkins_type,pos); + use_lastpose[switchkins_type] = 0; + } + + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !kfwds[switchkins_type]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: Forward BAD switchkins_type \n", + switchkins_type); + return -1; + } + r = kfwds[switchkins_type](joint, pos, fflags, iflags); + if (fwd_iterates[switchkins_type]) {save_lastpose(switchkins_type,pos);} + if (r) return r; + + // gui.* pins created only if gui_kinstype>=0 + // consider alternate implementations for gui_forward_kins(): + // a) always call and use -1 to select default 0 type + if (kp.gui_kinstype >=0) { + // create gui pins for a vismach gui using the + // kins type specified by kp.gui_kinstype; + // currently the skgui pins are only needed for + // the hexagui vismach program (as it needs + // world coords for switchkin-types + r = gui_forward_kins(joint); + } + + return r; +} // kinematicsForward() + +int kinematicsInverse(const EmcPose * pos, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + int r; + + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !kinvs[switchkins_type]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: Inverse BAD switchkins_type \n", + switchkins_type); + return -1; + } + r = kinvs[switchkins_type](pos, joint, iflags, fflags); + return r; +} // kinematicsInverse() + +KINEMATICS_TYPE kinematicsType() +{ + return KINEMATICS_BOTH; +} + +int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegister: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegister: switchkins-type %d" + " already provided\n", ktype); + register_error = 1; + return -1; + } + ksetups[ktype] = kset; + kfwds[ktype] = kfwd; + kinvs[ktype] = kinv; + return 0; +} // switchkinsRegister() + +EXPORT_SYMBOL(kinematicsSwitchable); +EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(switchkinsRegister); +EXPORT_SYMBOL(switchkinsInit); + +//********************************************************************* +// The caller owns the hal component: it does hal_init() before this and +// hal_ready() after it. Every switchkins-type must be registered by +// now. +int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates) +{ + int i; + int res = 0; + char* emsg = "other"; + + kp = *ksetup_parms; // kinematics parms are needed after this returns + + if (register_error) {emsg = "switchkinsRegister FAIL"; goto error;} + + // the highest type registered sets the count + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } + } + if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } + + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (kp.fwd_iterates_mask & (1< EMCMOT_MAX_JOINTS) { + emsg = "bogus max_joints"; goto error; + } + if (kp.gui_kinstype >= kins_count) { + emsg = "bogus gui_kinstype"; goto error; + } + + // a type left out below the highest one provided is a gap, not a count + for (i=0; i < kins_count; i++) { + if (ksetups[i] && kfwds[i] && kinvs[i]) { continue; } + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: switchkins-type %d incomplete:%s%s%s\n", + i, + ksetups[i] ? "" : " no setup", + kfwds[i] ? "" : " no forward", + kinvs[i] ? "" : " no inverse"); + emsg = "incomplete switchkins-type"; goto error; + } + + swdata = hal_malloc(sizeof(struct swdata)); + if (!swdata) {emsg = "hal_malloc fail"; goto error;} + + for (i=0; i < kins_count; i++) { + res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is[i]), + 0, "kinstype.is-%d", i); + } + + if (kp.gui_kinstype >=0) { + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_x, 0.0, "skgui.x"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_y, 0.0, "skgui.y"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_z, 0.0, "skgui.z"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_a, 0.0, "skgui.a"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_b, 0.0, "skgui.b"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_c, 0.0, "skgui.c"); + } + if (res) {emsg = "hal pin create fail"; goto error;} + + switchkins_type = 0; // startup with default type + kinematicsSwitch(switchkins_type); + + if (!coordinates) {coordinates = kp.required_coordinates;} + + for (i=0; i < kins_count; i++) { + ksetups[i](comp_id,coordinates,&kp); + } + + return 0; + +error: + rtapi_print_msg(RTAPI_MSG_ERR, + "\nSwitchkins FAIL %s:<%s>\n",kp.kinsname,emsg); + return -1; +} // switchkinsInit() diff --git a/src/Makefile b/src/Makefile index 9f33ba7a0d4..86cb2c8e5d1 100644 --- a/src/Makefile +++ b/src/Makefile @@ -402,6 +402,7 @@ SRCHEADERS := \ hal/drivers/mesa-hostmot2/hostmot2-serial.h \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ + emc/kinematics/switchkins.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/inifile.hh \ emc/ini/inifile.h \ diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 80c02613bb1..5fb1a12a42e 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -1,10 +1,10 @@ /* ** License GPL Version 2 */ -#ifndef SWITCHKINS_H // { -#define SWITCHKINS_H +#ifndef __LINUXCNC_SWITCHKINS_H +#define __LINUXCNC_SWITCHKINS_H -#include +#include "kinematics.h" //max number of switchkins types (KS,KF,KI) a module may provide: #define SWITCHKINS_MAX_TYPES 9 @@ -44,4 +44,4 @@ extern int switchkinsInit(const int comp_id, kparms* ksetup_parms, const char* coordinates ); -#endif // } +#endif diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 62c9940cfbb..d97a0baf2f1 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -94,11 +94,20 @@ endif obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, %.o, $(COMPS) $(COMP_DRIVERS))) +# A component that links objects besides its own names them here as +# -extra-objs. The list is expanded when the .mak is written, +# so it has to be defined in this file (which the .mak depends on). +SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +millturn-extra-objs := $(SWITCHKINS_OBJS) +xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) +xyzacb_trsrn-extra-objs := $(SWITCHKINS_OBJS) +xyzbca_trsrn-extra-objs := $(SWITCHKINS_OBJS) + objects/%.mak: %.comp hal/components/Submakefile $(ECHO) "Creating $(notdir $@)" @mkdir -p $(dir $@) - $(Q)echo $(notdir $*)-objs := objects/$*.o > $@.tmp - $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o >> $@.tmp + $(Q)echo $(notdir $*)-objs := objects/$*.o $($(notdir $*)-extra-objs) > $@.tmp + $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o $(addprefix objects/rt,$($(notdir $*)-extra-objs)) >> $@.tmp $(Q)mv -f $@.tmp $@ objects/%.c: %.comp ../bin/halcompile diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index dbf18cd3a1c..2768857fcd2 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -10,16 +10,15 @@ rotary axis. type1 is a turn (Z-YX) configuration with A configured to be a spindle. +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + For an example configuration, run the sim config: 'configs/sim/axis/vismach/millturn/millturn.ini'. Further explanations can be found in the README in 'configs/sim/axis/vismach/millturn'. -millturn.comp was constructed by modifying the template file: -userkins.comp. - -For more information on how to modify userkins.comp run: $ man -userkins. Also, see additional information inside: 'userkins.comp'. - For information on kinematics in general see the kinematics document chapter (docs/src/motion/kinematics.txt) and for switchable kinematics in particular see the switchkins document @@ -30,25 +29,16 @@ chapter (docs/src/motion/switchkins.txt) // Use the *_setup() function for pins and params used by kinematics. pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; +option extra_setup; function fdemo; license "GPL"; author "David Mueller"; ;; -#include - -static struct haldata { - // Example pin pointers: - hal_uint_t in; - hal_uint_t out; - // Example parameters: - //hal_real_t param_rw; - //hal_real_t param_ro; +#include - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; -} *haldata; +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -59,112 +49,30 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -static int millturn_setup(void) { -#define HAL_PREFIX "millturn" - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - // hal pin examples: - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->in, 0, "%s.in", HAL_PREFIX); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->out, 0, "%s.out", HAL_PREFIX); - // hal parameter examples: - //res += hal_param_new_real(comp_id, HAL_RW, &haldata->param_rw, 0.0, "%s.param-rw", HAL_PREFIX); - //res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> mill configuration - //-> turn configuration - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - - if (res) goto error; - hal_ready(comp_id); - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsSwitch(int new_switchkins_type) +// the turn kinematics need no hal pins of their own +static int turnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - return -1; // FAIL - } - return 0; // ok -} + (void)comp_id; + (void)coords; + (void)kp; + return 0; +} // turnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() -{ -static bool is_setup=0; - if (!is_setup) millturn_setup(); - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - -static bool is_ready=0; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int turnKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - static bool gave_msg; - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - break; - case 1: - pos->tran.x = j[2]; - pos->tran.y = -j[1]; - pos->tran.z = j[0]; - pos->a = j[3]; - break; - } + + pos->tran.x = j[2]; + pos->tran.y = -j[1]; + pos->tran.z = j[0]; + pos->a = j[3]; + // unused coordinates: pos->b = 0; pos->c = 0; @@ -172,46 +80,46 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (hal_get_ui32(haldata->in) && !is_ready && !gave_msg) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s the 'in' pin not echoed until Inverse called\n", - __FILE__); - gave_msg=1; - } return 0; -} // kinematicsForward() +} // turnKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turnKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH - - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - break; - case 1: - j[2] = pos->tran.x; - j[1] = -pos->tran.y; - j[0] = pos->tran.z; - j[3] = pos->a; - break; - } - - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference - //read from param example: *haldata->out = hal_get_real(haldata->param_rw); + + j[0] = pos->tran.z; + j[1] = -pos->tran.y; + j[2] = pos->tran.x; + j[3] = pos->a; return 0; -} // kinematicsInverse() +} // turnKinematicsInverse() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "millturn"; + kp.halprefix = "millturn"; + kp.required_coordinates = "xyza"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, turnKinematicsSetup, + turnKinematicsForward, + turnKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index ea9e39839a0..9aa9dc3fb19 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -13,16 +13,15 @@ axes XYZAB respectively. type1 is a XYZAB configuration with tool center point (TCP) compensation. +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + For an example configuration, run the sim config: '/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini'. Further explanations can be found in the README in '/configs/sim/axis/vismach/5axis/table-dual-rotary/'. -xyzab_tdr_kins.comp was constructed by modifying the template file: -userkins.comp. - -For more information on how to modify userkins.comp run: $ man -userkins. Also, see additional information inside: 'userkins.comp'. - For information on kinematics in general see the kinematics document chapter (docs/src/motion/kinematics.txt) and for switchable kinematics in particular see the switchkins document @@ -32,123 +31,68 @@ chapter (docs/src/motion/switchkins.txt) pin out si32 dummy=0"one pin needed to satisfy halcompile requirement"; +option extra_setup; + license "GPL"; author "David Mueller"; ;; #include -#include -static struct haldata { +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); - // Declare hal pin pointers used for xyzab_tdr kinematics: +static struct haldata { hal_real_t tool_offset_z; hal_real_t x_offset; hal_real_t z_offset; hal_real_t x_rot_point; hal_real_t y_rot_point; hal_real_t z_rot_point; +} *tdrdata; - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; -} *haldata; - -static int xyzab_tdr_setup(void) { -#define HAL_PREFIX "xyzab_tdr_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - // hal pins required for xyzab_tdr kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_offset, 0.0, "%s.z-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_point, 0.0, "%s.x-rot-point", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_point, 0.0, "%s.y-rot-point", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_point, 0.0, "%s.z-rot-point", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> XYZAB TCP - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - - if (res) goto error; - hal_ready(comp_id); - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsSwitch(int new_switchkins_type) +static int tdrKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - return -1; // FAIL - } - return 0; // ok -} - -KINEMATICS_TYPE kinematicsType() -{ -static bool is_setup=0; - if (!is_setup) xyzab_tdr_setup(); - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + int res = 0; + (void)coords; + + tdrdata = hal_malloc(sizeof(*tdrdata)); + if (!tdrdata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->tool_offset_z, 0.0, + "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_offset, 0.0, + "%s.z-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_rot_point, 0.0, + "%s.x-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->y_rot_point, 0.0, + "%s.y-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_rot_point, 0.0, + "%s.z-rot-point", kp->halprefix); + if (res) return -1; + + return 0; +} // tdrKinematicsSetup() +static int tdrKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -158,39 +102,22 @@ int kinematicsForward(const double *j, double cb = cos(j[4]*TO_RAD); // used to be consistent with math in the documentation - double px = 0; - double py = 0; - double pz = 0; - - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ====================== IDENTITY kinematics FORWARD ==================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - break; - case 1: // ========================= TCP kinematics FORWARD ====================== - px = j[0] - x_rot_point; - py = j[1] - y_rot_point; - pz = j[2] - z_rot_point - dt; - - pos->tran.x = cb*px + sb*pz - + x_rot_point; - - pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz - + y_rot_point; - - pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz - + z_rot_point + dz + dt; - - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - break; - } + double px = j[0] - x_rot_point; + double py = j[1] - y_rot_point; + double pz = j[2] - z_rot_point - dt; + + pos->tran.x = cb*px + sb*pz + + x_rot_point; + + pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz + + y_rot_point; + + pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz + + z_rot_point + dz + dt; + + pos->a = j[3]; + pos->b = j[4]; + // unused coordinates: pos->c = 0; pos->u = 0; @@ -198,22 +125,22 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // tdrKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdrKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dx = hal_get_real(haldata->x_offset); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dx = hal_get_real(tdrdata->x_offset); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -223,36 +150,46 @@ int kinematicsInverse(const EmcPose * pos, double cb = cos(pos->b*TO_RAD); // used to be consistent with math in the documentation - double qx = 0; - double qy = 0; - double qz = 0; - - switch (switchkins_type) { - case 0:// ====================== IDENTITY kinematics INVERSE ===================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - break; - case 1: // ========================= TCP kinematics INVERSE ====================== - qx = pos->tran.x - x_rot_point - dx; - qy = pos->tran.y - y_rot_point; - qz = pos->tran.z - z_rot_point - dz - dt; - - j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz - + x_rot_point; - - j[1] = ca*qy + sa*qz - + y_rot_point; - - j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz - + z_rot_point + dt; - - j[3] = pos->a; - j[4] = pos->b; - break; - } + double qx = pos->tran.x - x_rot_point - dx; + double qy = pos->tran.y - y_rot_point; + double qz = pos->tran.z - z_rot_point - dz - dt; + + j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz + + x_rot_point; + + j[1] = ca*qy + sa*qz + + y_rot_point; + + j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz + + z_rot_point + dt; + + j[3] = pos->a; + j[4] = pos->b; return 0; -} // kinematicsInverse() +} // tdrKinematicsInverse() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzab_tdr_kins"; + kp.halprefix = "xyzab_tdr_kins"; + kp.required_coordinates = "xyzab"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, tdrKinematicsSetup, + tdrKinematicsForward, + tdrKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index dfbe4466ace..4fb912e4e59 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -4,17 +4,26 @@ description """ FIXME +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + """; pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; +option extra_setup; license "GPL"; author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -35,122 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzacb_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -static int xyzacb_trsrn_setup(void) { -#define HAL_PREFIX "xyzacb_trsrn_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) +{ + int res = 0; + (void)coords; haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pins required for xyzacb_trsrn kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> xyzabc TCP - //-> xyzabc TOOL - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); - - if (res) goto error; - hal_ready(comp_id); - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,kp->halprefix); + if (res) return -1; -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - - - -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { -static bool is_setup=0; - if (!is_setup) xyzacb_trsrn_setup(); - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -195,20 +132,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -251,9 +175,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -291,10 +213,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -302,16 +220,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)fflags; (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() + +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ (void)fflags; + (void)iflags; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -359,23 +291,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -412,9 +328,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -456,9 +370,55 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; - - break; } return 0; -} // kinematicsInverse() +} // trsrnInverse() + +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzacb_trsrn"; + kp.halprefix = "xyzacb_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index b8f451c17f9..e5519f6ccbd 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -4,17 +4,26 @@ description """ FIXME +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + """; pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; +option extra_setup; license "GPL"; author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -35,122 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzbca_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -static int xyzbca_trsrn_setup(void) { -#define HAL_PREFIX "xyzbca_trsrn_kins" - int res=0; - // inbherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) +{ + int res = 0; + (void)coords; haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pins required for xyzbca_trsrn kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> xyzabc TCP - //-> xyzabc TOOL - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); - - if (res) goto error; - hal_ready(comp_id); - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", kp->halprefix); + if (res) return -1; -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - - - -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { -static bool is_setup=0; - if (!is_setup) xyzbca_trsrn_setup(); - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -196,20 +133,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -256,9 +180,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -296,10 +218,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -307,16 +225,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)fflags; (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() + +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ (void)fflags; + (void)iflags; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -362,23 +294,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -415,9 +331,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -459,9 +373,55 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; - - break; } return 0; -} // kinematicsInverse() +} // trsrnInverse() + +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzbca_trsrn"; + kp.halprefix = "xyzbca_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() From 3930f7d4e0caf54074f629262439608f5330f720 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 11:05:02 +1000 Subject: [PATCH 12/20] switchkins: add an out-of-tree module template Nothing stopped an out-of-tree kinematics module from using switchkins except that there was no way to get at the implementation, so anyone writing one reimplemented kinematicsSwitch() and the kinstype.is-N pins, or did without switching entirely. switchkinscomp.comp is the template for doing it properly. It sets TOPDIR to a source tree and includes switchkins.c and kins_util.c, which is how tpcomp.comp and homecomp.comp already reach the trajectory planning and homing sources. The module then registers its kinstypes and calls switchkinsInit() from EXTRA_SETUP(), the same fifteen lines the in-tree components use. That gets an out-of-tree module the kinematics switching, the kinstype.is-N pins, the coordinates= identity mapping and the HAL and G-code controls, all from the one implementation, and it costs no ABI: the sources are compiled into the module, so it is built against one tree and rebuilt when that tree changes. Like tpcomp, the template is not built in tree because it has no kinematics until TOPDIR is set, so it is filtered out of COMPS and its manpage is named explicitly. Renamed to user_switchkins, pointed at this tree and loaded as [KINS]KINEMATICS, it homes, switches to its example kinstype and back, and rejects a kinstype it does not have. --- docs/src/hal/components.adoc | 1 + docs/src/motion/switchkins.adoc | 48 +++++++ src/hal/components/Submakefile | 8 +- src/hal/components/switchkinscomp.comp | 167 +++++++++++++++++++++++++ 4 files changed, 220 insertions(+), 4 deletions(-) create mode 100644 src/hal/components/switchkinscomp.comp diff --git a/docs/src/hal/components.adoc b/docs/src/hal/components.adoc index 8e04a648a93..ace77ef56de 100644 --- a/docs/src/hal/components.adoc +++ b/docs/src/hal/components.adoc @@ -338,6 +338,7 @@ Limit its slew rate to less than maxv per second. Limit its second derivative to | link:../man/man9/rosekins.9.html[rosekins] |Kinematics for a rose engine || | link:../man/man9/rotatekins.9.html[rotatekins] |The X and Y axes are rotated 45 degrees compared to the joints 0 and 1. || | link:../man/man9/scarakins.9.html[scarakins] |Kinematics for SCARA-type robots. || +| link:../man/man9/switchkinscomp.9.html[switchkinscomp] |Switchable kinematics module template || | link:../man/man9/kins.9.html[three21kins] |Analytical kinematics solver for 6-DOF arm + wrist robots. || | link:../man/man9/tripodkins.9.html[tripodkins] |The joints represent the distance of the controlled point from three predefined locations (the motors), giving three degrees of freedom in position (XYZ). || | link:../man/man9/userkins.9.html[userkins] |Template for user-built kinematics || diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 18f1eab0d36..7f726b1193c 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -406,6 +406,12 @@ configs/sim/axis/vismach/ . == User kinematics provisions +There are two ways to supply custom kinematics. Adding a kinstype to +a module that is already in the tree is the smaller job; building a +module of your own gives you every kinstype it provides. + +=== Adding a kinstype to an in-tree module + Custom kinematics can be coded and tested on Run-In-Place ('RIP') builds. A template file src/emc/kinematics/userkfuncs.c is provided in the distribution. This file can be copied/renamed to a user @@ -423,6 +429,47 @@ Preempt-rt make example: $ userkfuncs=/home/myname/kins/mykins.c make && sudo make setuid ---- +=== Building a switchkins module of your own + +A complete kinematics module can be built out-of-tree with halcompile +using the same switchkins implementation the in-tree modules use, so +it gets the kinematics switching, the 'kinstype.is-N' pins, the +'coordinates=' identity mapping and the G-code and HAL controls +without reimplementing any of them. + +The template is src/hal/components/switchkinscomp.comp. Copy and +rename it (both the file and the component name), point its TOPDIR +at a LinuxCNC source tree, and replace the example kinstype with the +real kinematics: + +[source,c] +---- +#define TOPDIR /home/myname/linuxcnc-dev +// ... +#include USE_TOPDIR(src/emc/kinematics/switchkins.c) +#include USE_TOPDIR(src/emc/kinematics/kins_util.c) +---- + +The module registers each of its kinstypes and calls switchkinsInit() +from EXTRA_SETUP(), which halcompile runs after hal_init() and before +hal_ready(). See <> for both +calls. + +---- +$ halcompile --install user_switchkins.comp +---- + +[source,ini] +---- +[KINS] +KINEMATICS = user_switchkins +JOINTS = 3 +---- + +[NOTE] +The switchkins sources are compiled into the module, so it is built +against one source tree and has to be rebuilt when that tree changes. + == Warnings Unexpected behavior can result if a G-code program is inadvertently @@ -447,6 +494,7 @@ The management of coordinate offsets, tool compensation, and INI file limits may require complicated and non-standard operating protocols. +[[sec:switchkins-code-notes]] == Code Notes Kinematic modules providing switchkins functionality are linked to diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index d97a0baf2f1..8ad4ee1740e 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -1,5 +1,5 @@ ifneq ($(KERNELRELEASE),) -COMPS := $(filter-out %/tpcomp.comp, $(patsubst $(BASEPWD)/%,%,$(wildcard $(BASEPWD)/hal/components/*.comp $(BASEPWD)/hal/drivers/*.comp))) +COMPS := $(filter-out %/tpcomp.comp %/switchkinscomp.comp, $(patsubst $(BASEPWD)/%,%,$(wildcard $(BASEPWD)/hal/components/*.comp $(BASEPWD)/hal/drivers/*.comp))) include $(patsubst %.comp, $(BASEPWD)/objects/%.mak, $(COMPS)) else CONVERTERS := \ @@ -32,8 +32,8 @@ CONVERTERS := \ conv_u64_s32.comp \ conv_u64_u32.comp \ conv_u64_s64.comp -COMPS := $(filter-out hal/components/tpcomp.comp, $(sort $(wildcard hal/components/*.comp) $(addprefix hal/components/, $(CONVERTERS)))) -COMP_MANPAGES := $(patsubst hal/components/%.comp, ../docs/build/man/man9/%.9, $(COMPS)) ../docs/build/man/man9/tpcomp.9 +COMPS := $(filter-out hal/components/tpcomp.comp hal/components/switchkinscomp.comp, $(sort $(wildcard hal/components/*.comp) $(addprefix hal/components/, $(CONVERTERS)))) +COMP_MANPAGES := $(patsubst hal/components/%.comp, ../docs/build/man/man9/%.9, $(COMPS)) ../docs/build/man/man9/tpcomp.9 ../docs/build/man/man9/switchkinscomp.9 ifeq ($(BUILD_SYS),uspace) COMP_DRIVERS += hal/drivers/serport.comp COMP_DRIVERS += hal/drivers/mesa_7i65.comp @@ -58,7 +58,7 @@ endif # wildcard that mixes hal/components and hal/drivers, so deriving the adoc # targets from it there yields hal/drivers/*.comp entries that fail the # hal/components/%.comp static pattern rule. -COMP_MANPAGE_ADOCS := $(patsubst hal/components/%.comp, objects/man/man9/%.9.adoc, $(COMPS)) objects/man/man9/tpcomp.9.adoc +COMP_MANPAGE_ADOCS := $(patsubst hal/components/%.comp, objects/man/man9/%.9.adoc, $(COMPS)) objects/man/man9/tpcomp.9.adoc objects/man/man9/switchkinscomp.9.adoc COMP_DRIVER_MANPAGE_ADOCS := $(patsubst hal/drivers/%.comp, objects/man/man9/%.9.adoc, $(COMP_DRIVERS)) # Extract adoc from .comp via halcompile --adoc. Only needs Python + diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp new file mode 100644 index 00000000000..1d9fbdbe6d7 --- /dev/null +++ b/src/hal/components/switchkinscomp.comp @@ -0,0 +1,167 @@ +component switchkinscomp "switchable kinematics module template"; +// NOTE: component name must agree with filename + +description """ +Example of a switchable kinematics module buildable with halcompile. + +The switchkinscomp.comp file (src/hal/components/switchkinscomp.comp) +illustrates a method to use halcompile to build a kinematics module +on top of the switchkins implementation used by the in-tree kinematics +modules, so an out-of-tree module gets the same kinematics switching, +the same 'kinstype.is-N' pins, the same 'coordinates=' identity +mapping, and the same G-code and HAL controls, without reimplementing +any of it. + +The example switchkinscomp.comp is not usable until modified for the +user environment. To create a runnable switchkinscomp module, the +file must be edited to supply a valid '#define TOPDIR' pointing at a +LinuxCNC source tree. + +To avoid updates that overwrite switchkinscomp.comp, best practice is +to rename the file and its component name (example: +*user_switchkins.comp* creates module: *user_switchkins*). + +The (renamed) component can be built and installed with halcompile +and then used as the kinematics module by inifile setting: + +[source,ini] +---- +[KINS] +KINEMATICS = user_switchkins +JOINTS = 3 +---- + +*Note:* If using a deb install: + +1. halcompile is provided by the deb package linuxcnc-dev +2. This source file for BRANCHNAME (master, 2.9, etc) is downloadable from github: + +https://github.com/LinuxCNC/linuxcnc/blob/BRANCHNAME/src/hal/components/switchkinscomp.comp + +For information on switchable kinematics see the switchkins document +chapter (docs/src/motion/switchkins.txt). +"""; + +pin out bit is_module=1; //one pin is required to use halcompile + +license "GPL"; +option extra_setup; +;; + +//===================================================================== +/* To use the switchkins implementation from a local git src tree: +** set TOPDIR to the git tree top directory +** (Edit 'myname' as required) +*/ + +//#define TOPDIR /home/myname/linuxcnc-dev + +#ifdef TOPDIR // { + +#define STR(s) #s +#define XSTR(s) STR(s) +#define USE_TOPDIR(b) XSTR(TOPDIR/b) + +// switchkins.c provides kinematicsForward(), kinematicsInverse(), +// kinematicsSwitch() and the rest of the kinematics interface, and +// dispatches each call to the currently selected switchkins-type. +// kins_util.c provides the identity kinematics and the coordinates +// letters-to-joints mapping they use. +#include USE_TOPDIR(src/emc/kinematics/switchkins.c) +#include USE_TOPDIR(src/emc/kinematics/kins_util.c) + +#else +#error No TOPDIR defined, skeleton component provides no kinematics functions. +#endif // } +//===================================================================== + +// module parameter naming the joint order for the identity type +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +//--------------------------------------------------------------------- +// Example switchkins-type. A setup routine creating whatever hal pins +// the kinematics need, plus a forward and an inverse routine. Replace +// the arithmetic with the real kinematics. + +static struct { + hal_real_t x_offset; +} *mydata; + +static int myKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) +{ + (void)coords; // this type does not use the coordinates mapping + + mydata = hal_malloc(sizeof(*mydata)); + if (!mydata) return -1; + + return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); +} // myKinematicsSetup() + +static int myKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + + pos->tran.x = j[0] + hal_get_real(mydata->x_offset); + pos->tran.y = j[1]; + pos->tran.z = j[2]; + + // unused coordinates: + pos->a = pos->b = pos->c = 0; + pos->u = pos->v = pos->w = 0; + + return 0; +} // myKinematicsForward() + +static int myKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + + j[0] = pos->tran.x - hal_get_real(mydata->x_offset); + j[1] = pos->tran.y; + j[2] = pos->tran.z; + + return 0; +} // myKinematicsInverse() + +//--------------------------------------------------------------------- +// rtapi_app_main() is supplied by halcompile, which calls hal_init() +// before EXTRA_SETUP() and hal_ready() after it. That is what +// switchkinsInit() expects, so the switchkins-types are registered and +// the implementation started from here. + +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "switchkinscomp"; // must agree with the module name + kp.halprefix = "switchkinscomp"; // hal pin names + kp.required_coordinates = "xyz"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; // set bit N if type N iterates + kp.gui_kinstype = -1; // negative means: not used + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + // switchkins-type 0 is the startup default. Types run from 0 to + // SWITCHKINS_MAX_TYPES-1 with no gaps. + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, myKinematicsSetup, + myKinematicsForward, + myKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() From b9e14759e22d9b70fb05130e9143c5e39a484f36 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 19:13:27 +1000 Subject: [PATCH 13/20] kins: drop the kinematics.h include switchkins.h already provides switchkins.h includes kinematics.h, so a module that includes switchkins.h does not need to include kinematics.h itself. switchkins.c had picked up the habit along with genhexkins, 5axiskins, pumakins, scarakins and three21kins, which had it before any of this. Modules that do not use switchkins.h still include kinematics.h directly, as they must. --- src/emc/kinematics/5axiskins.c | 1 - src/emc/kinematics/genhexkins.c | 1 - src/emc/kinematics/pumakins.c | 1 - src/emc/kinematics/scarakins.c | 1 - src/emc/kinematics/switchkins.c | 1 - src/emc/kinematics/three21kins.c | 1 - 6 files changed, 6 deletions(-) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 387c32e23df..55abdb22184 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -59,7 +59,6 @@ #include #include #include -#include #include "switchkins.h" diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 3cddd9a72bc..8adfb17b47b 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -110,7 +110,6 @@ #include #include #include -#include /* these decls, KINEMATICS_FORWARD_FLAGS */ #include "genhexkins.h" #include "switchkins.h" diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index f055e73a502..5f5a1a53966 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -20,7 +20,6 @@ #include #include #include -#include #include "pumakins.h" #include "switchkins.h" diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 2454a67bfb2..8eaf024aabd 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -19,7 +19,6 @@ #include #include #include -#include #include "switchkins.h" diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 68cf8e36e25..7a3f3d35071 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -29,7 +29,6 @@ #include #include #include -#include #include "switchkins.h" diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 219f3877427..0ae19e1f2c7 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -2,7 +2,6 @@ #include #include #include -#include #include "switchkins.h" From 021c4621793086ffc228914a44190217e9777af0 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 21:02:46 +1000 Subject: [PATCH 14/20] kins: include switchkins.h as an exported header The kinematics modules are users of switchkins, not part of it, so they take the header the way any other user would. switchkins.c and switchkins_main.c keep the quoted form, being the source itself. --- src/emc/kinematics/5axiskins.c | 2 +- src/emc/kinematics/genhexkins.c | 2 +- src/emc/kinematics/genserkins.c | 2 +- src/emc/kinematics/pumakins.c | 2 +- src/emc/kinematics/scarakins.c | 2 +- src/emc/kinematics/three21kins.c | 2 +- src/emc/kinematics/xyzac-trt-kins.c | 2 +- src/emc/kinematics/xyzbc-trt-kins.c | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 55abdb22184..5f2efb7d216 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -60,7 +60,7 @@ #include #include -#include "switchkins.h" +#include static struct haldata { hal_real_t pivot_length; diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 8adfb17b47b..978948dc463 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -112,7 +112,7 @@ #include #include "genhexkins.h" -#include "switchkins.h" +#include static struct haldata { hal_real_t basex[NUM_STRUTS]; diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index 64fe55983e1..9e6284d5513 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -42,7 +42,7 @@ frame-larger-than: #include #include "genserkins.h" -#include "switchkins.h" +#include //-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) #undef GO_REAL_EPSILON diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 5f5a1a53966..602a9acca37 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -22,7 +22,7 @@ #include #include "pumakins.h" -#include "switchkins.h" +#include struct haldata { hal_real_t a2, a3, d3, d4, d6; diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 8eaf024aabd..a36fafbfc9b 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -20,7 +20,7 @@ #include #include -#include "switchkins.h" +#include static struct scara_data { hal_real_t d1, d2, d3, d4, d5, d6; diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 0ae19e1f2c7..abc346b33db 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -3,7 +3,7 @@ #include #include -#include "switchkins.h" +#include /* default values for ar2 robot */ #define DEFAULT_THREE21_A1 64.2 diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 47655ec0f14..c818ae4075a 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index aa1289baf28..142de97312c 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, From d3afe5e7f7b2a49ba32f3db68c9bcb4e5c22b91f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 23:51:35 +1000 Subject: [PATCH 15/20] switchkins: install the implementation as source for out-of-tree modules A realtime module cannot link a library, so an out-of-tree kinematics module has to compile the switchkins implementation itself. Asking it for the path to a source tree, as the template did, leaves anybody on a deb install with nothing to point at. Install switchkins.c and kins_util.c into share/linuxcnc, the way mesa_modbus.c.tmpl already is, and put that directory on the realtime include path. The template then reads #include #include and builds as it stands. --- .gitignore | 2 ++ debian/linuxcnc-uspace-dev.install | 2 ++ docs/src/motion/switchkins.adoc | 17 +++++++------ src/Makefile | 1 + src/Makefile.modinc.in | 4 +-- src/emc/kinematics/Submakefile | 13 ++++++++++ src/hal/components/switchkinscomp.comp | 34 ++++++++------------------ 7 files changed, 40 insertions(+), 33 deletions(-) diff --git a/.gitignore b/.gitignore index 4e3ccbcb4c8..18eb2868130 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,8 @@ share/desktop-directories/linuxcnc-cnc.directory share/desktop-directories/linuxcnc-ref.directory share/desktop-directories/linuxcnc-doc.directory share/linuxcnc/mesa_modbus.c.tmpl +share/linuxcnc/switchkins.c +share/linuxcnc/kins_util.c src/modules.order /configs/*/emc.nml !/configs/common/emc.nml diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index 199dae9fcc0..39c124d3532 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -5,3 +5,5 @@ usr/lib/liblinuxcnc.a usr/lib/*.so usr/share/linuxcnc/Makefile.modinc usr/share/linuxcnc/mesa_modbus.c.tmpl +usr/share/linuxcnc/switchkins.c +usr/share/linuxcnc/kins_util.c diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 7f726b1193c..a81b0dd01d4 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -438,18 +438,21 @@ it gets the kinematics switching, the 'kinstype.is-N' pins, the without reimplementing any of them. The template is src/hal/components/switchkinscomp.comp. Copy and -rename it (both the file and the component name), point its TOPDIR -at a LinuxCNC source tree, and replace the example kinstype with the -real kinematics: +rename it (both the file and the component name) and replace the +example kinstype with the real kinematics. The implementation itself +is included: [source,c] ---- -#define TOPDIR /home/myname/linuxcnc-dev -// ... -#include USE_TOPDIR(src/emc/kinematics/switchkins.c) -#include USE_TOPDIR(src/emc/kinematics/kins_util.c) +#include +#include ---- +A realtime module cannot link a library, so the implementation arrives +as source: switchkins.c and kins_util.c are installed beside the +headers, in share/linuxcnc, and halcompile already looks there. With +a deb install they come from the linuxcnc-dev package. + The module registers each of its kinstypes and calls switchkinsInit() from EXTRA_SETUP(), which halcompile runs after hal_init() and before hal_ready(). See <> for both diff --git a/src/Makefile b/src/Makefile index 86cb2c8e5d1..61799d00ea8 100644 --- a/src/Makefile +++ b/src/Makefile @@ -774,6 +774,7 @@ ifeq ($(BUILD_GUI),yes) endif $(FILE) ../src/hal/drivers/mesa-hostmot2/modbus/*.tmpl $(DESTDIR)$(prefix)/share/linuxcnc/ + $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c $(DESTDIR)$(prefix)/share/linuxcnc/ install-kernel-indep: install-python install-python: install-dirs diff --git a/src/Makefile.modinc.in b/src/Makefile.modinc.in index ed9d75d98c2..cfcf1bc0b7d 100644 --- a/src/Makefile.modinc.in +++ b/src/Makefile.modinc.in @@ -76,12 +76,12 @@ EXTRA_CFLAGS += -fno-builtin-sin -fno-builtin-cos -fno-builtin-sincos EMC2_HOME=@EMC2_HOME@ RUN_IN_PLACE=@RUN_IN_PLACE@ ifeq ($(RUN_IN_PLACE),yes) -EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I$(EMC2_HOME)/include +EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I$(EMC2_HOME)/include -I$(EMC2_HOME)/share/linuxcnc RTLIBDIR := @EMC2_HOME@/rtlib LIBDIR := @EMC2_HOME@/lib else prefix := @prefix@ -EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I@includedir@/linuxcnc +EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I@includedir@/linuxcnc -I${prefix}/share/linuxcnc RTLIBDIR := @EMC2_RTLIB_DIR@ LIBDIR := @libdir@ endif diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 77085c21c2e..7e2f2d84b4b 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -33,3 +33,16 @@ $(RDELTAMODULE): $(call TOOBJS, $(RDELTAMODULESRCS)) $(ECHO) Linking python module $(notdir $@) $(CXX) $(LDFLAGS) -shared -o $@ $^ $(BOOST_PYTHON_LIB) PYTARGETS += $(RDELTAMODULE) + +# The switchkins implementation is shipped as source, since a realtime module +# cannot link a library, so a module built out of tree includes it the way the +# in-tree ones link it. +EMCKINEMATICSSRCS = \ + ../share/linuxcnc/switchkins.c \ + ../share/linuxcnc/kins_util.c + +$(EMCKINEMATICSSRCS): ../share/linuxcnc/%.c: ./emc/kinematics/%.c + $(ECHO) Copying switchkins source $(notdir $@) + $(Q)cp -f $< $@ + +TARGETS += $(EMCKINEMATICSSRCS) diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index 1d9fbdbe6d7..7e90edc380f 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -12,10 +12,10 @@ the same 'kinstype.is-N' pins, the same 'coordinates=' identity mapping, and the same G-code and HAL controls, without reimplementing any of it. -The example switchkinscomp.comp is not usable until modified for the -user environment. To create a runnable switchkinscomp module, the -file must be edited to supply a valid '#define TOPDIR' pointing at a -LinuxCNC source tree. +The example builds as it stands, its type 1 being an X offset to +replace with the kinematics wanted. The switchkins implementation is +installed as source alongside the headers, so nothing needs a path to +a LinuxCNC source tree. To avoid updates that overwrite switchkinscomp.comp, best practice is to rename the file and its component name (example: @@ -33,7 +33,8 @@ JOINTS = 3 *Note:* If using a deb install: -1. halcompile is provided by the deb package linuxcnc-dev +1. halcompile and the switchkins source are provided by the deb + package linuxcnc-dev 2. This source file for BRANCHNAME (master, 2.9, etc) is downloadable from github: https://github.com/LinuxCNC/linuxcnc/blob/BRANCHNAME/src/hal/components/switchkinscomp.comp @@ -49,30 +50,15 @@ option extra_setup; ;; //===================================================================== -/* To use the switchkins implementation from a local git src tree: -** set TOPDIR to the git tree top directory -** (Edit 'myname' as required) -*/ - -//#define TOPDIR /home/myname/linuxcnc-dev - -#ifdef TOPDIR // { - -#define STR(s) #s -#define XSTR(s) STR(s) -#define USE_TOPDIR(b) XSTR(TOPDIR/b) - // switchkins.c provides kinematicsForward(), kinematicsInverse(), // kinematicsSwitch() and the rest of the kinematics interface, and // dispatches each call to the currently selected switchkins-type. // kins_util.c provides the identity kinematics and the coordinates -// letters-to-joints mapping they use. -#include USE_TOPDIR(src/emc/kinematics/switchkins.c) -#include USE_TOPDIR(src/emc/kinematics/kins_util.c) +// letters-to-joints mapping they use. Both are installed with the +// headers, so halcompile finds them with no path of your own. -#else -#error No TOPDIR defined, skeleton component provides no kinematics functions. -#endif // } +#include +#include //===================================================================== // module parameter naming the joint order for the identity type From 311fd411046c86d5b6ccfbc22fc2436d7a16eace Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 25 Aug 2026 01:03:33 +1000 Subject: [PATCH 16/20] canon: stop printing on every kinematics switch gcodemodule.cc got a raw printf when SELECT_KINS_TYPE was added, so the preview printed a line for every G12.1 and G13.1 in the program. It is the only live printf in the file, every other one having been commented out, and the neighbouring canon stubs are empty. Make this one empty too. saicanon.cc had the same printf. There it should report, since saicanon exists to echo the canonical commands, but through the same macro as the rest of the file so it lands in the canon output with a line number and the argument rather than beside it on stdout. --- src/emc/rs274ngc/gcodemodule.cc | 8 +------- src/emc/sai/saicanon.cc | 5 +---- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 6ead6b3b746..c935a9f5492 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -890,13 +890,7 @@ void ON_RESET() {} void PALLET_SHUTTLE() {} void SELECT_TOOL(int tool) {selected_tool = tool;} void UPDATE_TAG(const StateTag& /*tag*/) {} -void SELECT_KINS_TYPE(int switchkins_type) -{ - (void)switchkins_type; - printf("gcodemodule: SELECT_KINS_TYPE\n"); - - return; -} +void SELECT_KINS_TYPE(int /*switchkins_type*/) {} void OPTIONAL_PROGRAM_STOP() {} int GET_EXTERNAL_TC_FAULT() {return 0;} int GET_EXTERNAL_TC_REASON() {return 0;} diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 73af2751dd4..e1462fe2529 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -1199,8 +1199,5 @@ void UPDATE_TAG(const StateTag& /*tag*/){ void SELECT_KINS_TYPE(int switchkins_type) { - (void)switchkins_type; - printf("saicanon: SELECT_KINS_TYPE\n"); - - return; + ECHO_WITH_ARGS("%d", switchkins_type); } From 52d1b62d06be292a4a8a07424dfec5a518700c3a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 10 Aug 2026 18:05:21 +1000 Subject: [PATCH 17/20] kinematics: evaluate a module outside RT by binding to its live pins Alternative to publishing a parameter snapshot in shared memory. A kinematics module's haldata is a struct of pin handles, and a handle is an opaque pointer to the value cell that hal_get_real() reads. So a second, non-RT copy of the module can point its haldata at the cells the running RT instance already owns, and then its forward and inverse work unmodified, on live values, at any pose the caller asks for. Per module that comes to: - export nonrt_attach(), which asks a caller-supplied resolver for each of its input pins by name, runs the same coordinate parse setup runs, and returns the module's existing forward and inverse pointers; - split the coordinate parse out of setup so nonrt_attach() can call it without creating pins. Nothing else changes. The kinematics math is untouched, there is no parameter struct to declare, no shared header that grows once per module, no snapshot to refresh from inside the servo loop and no sequence counter to get right. Out-of-tree modules can opt in without anyone editing a header they do not own. The module never looks a pin up itself. Name lookup lives in the loader, which is ordinary userspace code linked against liblinuxcnchal and can therefore use hal_getref_p(); the module is built as an RT object, and walking the HAL name space from an RT object is exactly what the HAL isolation work is removing. Keeping the lookup on the caller's side also avoids binding the module against the wrong copy of the HAL symbols, since rtlib's hal_lib.so and liblinuxcnchal.so export the same names. No HAL change is needed: this patch touches no file under src/hal. hal_getref_p() resolves a netted pin to the signal's cell, which is not an edge case. In the bridgemill sim config 5axiskins.pivot-length is driven from the tool offset, so a bound copy has to follow the signal to see the right geometry. Bind input pins only. Output pins and scratch storage stay private to the non-RT copy: the two copies run in different processes and must not write to each other's state. 5axiskins has neither, so its non-RT haldata is one static struct. trivkins needs no binding at all, only a statement that joints are axes. A reference stays valid while the owning component is loaded and nothing invalidates it afterwards, so a caller must not outlive the kinematics module it bound to. Verified against the struct-snapshot version of this work: for the same pivot length, kinslimits reports identical caps to the digit. With 5axiskins.pivot-length set by setp and no motion thread running, this version reads the value that was set and the snapshot version reads the setup default, because a snapshot only refreshes when RT calls forward or inverse. With the pin netted from a signal, the bound copy tracks the signal. --- debian/linuxcnc.install.in | 1 + src/Makefile | 3 + src/emc/kinematics/5axiskins.c | 63 ++- src/emc/kinematics/nonrt_kins.h | 103 +++++ src/emc/kinematics/trivkins.c | 15 + src/emc/kinematics_userspace/Submakefile | 1 + .../kinematics_userspace/kinematics_user.c | 282 ++++++++++++++ .../kinematics_userspace/kinematics_user.h | 187 +++++++++ src/emc/motion_planning/Submakefile | 46 +++ src/emc/motion_planning/jacobian.cc | 197 ++++++++++ src/emc/motion_planning/jacobian.hh | 104 +++++ src/emc/motion_planning/joint_limits.cc | 358 ++++++++++++++++++ src/emc/motion_planning/joint_limits.hh | 238 ++++++++++++ src/emc/motion_planning/kinslimits.cc | 268 +++++++++++++ 14 files changed, 1857 insertions(+), 9 deletions(-) create mode 100644 src/emc/kinematics/nonrt_kins.h create mode 100644 src/emc/kinematics_userspace/Submakefile create mode 100644 src/emc/kinematics_userspace/kinematics_user.c create mode 100644 src/emc/kinematics_userspace/kinematics_user.h create mode 100644 src/emc/motion_planning/Submakefile create mode 100644 src/emc/motion_planning/jacobian.cc create mode 100644 src/emc/motion_planning/jacobian.hh create mode 100644 src/emc/motion_planning/joint_limits.cc create mode 100644 src/emc/motion_planning/joint_limits.hh create mode 100644 src/emc/motion_planning/kinslimits.cc diff --git a/debian/linuxcnc.install.in b/debian/linuxcnc.install.in index d66045b43a5..a71302665a0 100644 --- a/debian/linuxcnc.install.in +++ b/debian/linuxcnc.install.in @@ -36,6 +36,7 @@ usr/bin/hy_vfd usr/bin/image-to-gcode usr/bin/inivalue usr/bin/inivar +usr/bin/kinslimits usr/bin/latency-histogram usr/bin/latency-plot usr/bin/latency-test diff --git a/src/Makefile b/src/Makefile index 61799d00ea8..37f0c6664ad 100644 --- a/src/Makefile +++ b/src/Makefile @@ -193,6 +193,7 @@ SUBDIRS := \ \ $(GUI_SUBDIRS) \ emc/usr_intf/axis emc/usr_intf emc/nml_intf emc/task emc/kinematics emc/canterp \ + emc/motion_planning emc/kinematics_userspace \ emc/ini emc/rs274ngc emc/sai emc/pythonplugin \ emc/motion-logger \ emc/tooldata \ @@ -403,6 +404,8 @@ SRCHEADERS := \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ emc/kinematics/switchkins.h \ + emc/kinematics/nonrt_kins.h \ + emc/kinematics_userspace/kinematics_user.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/inifile.hh \ emc/ini/inifile.h \ diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 5f2efb7d216..16bbf285628 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -61,6 +61,7 @@ #include #include +#include static struct haldata { hal_real_t pivot_length; @@ -158,11 +159,20 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, return 0; } // fiveaxis_kinematicsInverse() -int fiveaxis_KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) +// module constants, shared by switchkinsSetup() and nonrt_attach() +static void fiveaxis_kparms(kparms* kp) +{ + kp->kinsname = "5axiskins"; // !!! must agree with filename + kp->halprefix = "5axiskins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; +} + +// assign principal joint numbers from the coordinates string. +// No HAL involvement, so the non-RT path can use it too. +static int fiveaxis_map_joints(const char* coordinates, kparms* kp) { - int result=0; int i,jno; int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; int minjoints = strlen(kp->required_coordinates); @@ -211,6 +221,20 @@ int fiveaxis_KinematicsSetup(const int comp_id, if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;} } + return 0; + +error: + return -1; +} // fiveaxis_map_joints() + +int fiveaxis_KinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + int result=0; + + if (fiveaxis_map_joints(coordinates, kp)) goto error; + haldata = hal_malloc(sizeof(*haldata)); if(!haldata) goto error; @@ -239,11 +263,7 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { - kp->kinsname = "5axiskins"; // !!! must agree with filename - kp->halprefix = "5axiskins"; // hal pin names - kp->required_coordinates = REQUIRED_COORDINATES; - kp->allow_duplicates = 1; - kp->max_joints = EMCMOT_MAX_JOINTS; + fiveaxis_kparms(kp); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); @@ -270,3 +290,28 @@ int switchkinsSetup(kparms* kp, return 0; } // switchkinsSetup() + +// Non-RT entry point: bind this copy of the module to the pins the +// running RT instance owns, then hand back the unmodified kinematics. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + static struct haldata nonrt_haldata; // private to this copy of the module + kparms kp = {0}; + + fiveaxis_kparms(&kp); + + haldata = &nonrt_haldata; + + if (nonrt_resolve_real(resolve, arg, &haldata->pivot_length, + "%s.pivot-length", kp.halprefix)) return -1; + + if (fiveaxis_map_joints(coordinates, &kp)) return -1; + + ops->forward = fiveaxis_KinematicsForward; + ops->inverse = fiveaxis_KinematicsInverse; + ops->is_identity = 0; + return 0; +} // nonrt_attach() + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/nonrt_kins.h b/src/emc/kinematics/nonrt_kins.h new file mode 100644 index 00000000000..7b01e304d5d --- /dev/null +++ b/src/emc/kinematics/nonrt_kins.h @@ -0,0 +1,103 @@ +/******************************************************************** + * Description: nonrt_kins.h + * Interface a kinematics module exports so that a non-RT caller can + * evaluate it. + * + * A trajectory planner needs forward and inverse kinematics at poses + * the machine has not reached yet, which means calling them outside + * the servo thread. A module opts in by exporting nonrt_attach(). + * + * The caller dlopens the module and calls nonrt_attach() once with + * the coordinates string from the INI file and a resolver callback. + * The module asks the resolver for each of its input pins by name, + * stores the returned references in its own haldata, and hands back + * its existing forward and inverse entry points. Both copies then + * read the same value cells and the kinematics code itself does not + * change. + * + * The module never looks a pin up itself. Name lookup belongs to + * the caller, which is ordinary userspace code linked against + * liblinuxcnchal; this file is compiled as part of an RT module, and + * an RT module has no business walking the HAL name space. Keeping + * the lookup on the caller's side is also what stops the module from + * binding against the wrong copy of the HAL symbols, since rtlib's + * hal_lib.so and liblinuxcnchal.so export the same names. + * + * Resolve input pins only. Output pins and any scratch storage must + * stay private to the non-RT copy, or the two copies will write to + * each other's state. + * + * A reference stays valid for as long as the component that owns the + * pin is loaded. Nothing invalidates it if that component goes away, + * so a caller must not outlive the kinematics module it bound to. + * + * Modules that do not export nonrt_attach() still work as they + * always have; the caller sees that the symbol is missing and does + * without kinematic limits for that machine. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#ifndef NONRT_KINS_H +#define NONRT_KINS_H + +#include + +#include +#include +#include +#include + +/* Supplied by the caller. Looks 'pin_name' up in HAL, checks that it + has type 'type', and writes its value reference to 'out'. A netted + pin must resolve to the signal's cell, not the pin's own. + Returns 0 on success. */ +typedef int (*nonrt_resolve_fn)(const char *pin_name, + hal_type_t type, + hal_refs_u *out, + void *arg); + +/* Filled in by nonrt_attach(). A module that reports is_identity has + joints equal to axes and the caller needs no module code at all, so + forward and inverse may be left NULL. */ +typedef struct { + int (*forward)(const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + int (*inverse)(const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + int is_identity; +} nonrt_ops_t; + +/* Exported by a participating module: + int nonrt_attach(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + Returns 0 on success. */ + +/* Convenience for the common case: resolve one float pin, by printf + style name, into a haldata field. */ +static inline int nonrt_resolve_real(nonrt_resolve_fn resolve, void *arg, + hal_real_t *dst, const char *fmt, ...) +{ + char name[HAL_NAME_LEN + 1]; + hal_refs_u ref; + va_list ap; + + if (!resolve || !dst) return -1; + + va_start(ap, fmt); + rtapi_vsnprintf(name, sizeof(name), fmt, ap); + va_end(ap); + + if (resolve(name, HAL_FLOAT, &ref, arg) != 0) return -1; + + *dst = ref.r; + return 0; +} + +#endif /* NONRT_KINS_H */ diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 3ea56b49aa8..848f7fa7918 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -18,6 +18,7 @@ #include #include #include +#include "nonrt_kins.h" #define SET(f) pos->f = joints[i] @@ -85,3 +86,17 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +// Non-RT entry point: joints are axes, so a non-RT caller needs no +// module code at all and reads nothing from HAL. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + (void)coordinates; (void)resolve; (void)arg; + ops->forward = NULL; + ops->inverse = NULL; + ops->is_identity = 1; + return 0; +} + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics_userspace/Submakefile b/src/emc/kinematics_userspace/Submakefile new file mode 100644 index 00000000000..f92b17d356e --- /dev/null +++ b/src/emc/kinematics_userspace/Submakefile @@ -0,0 +1 @@ +INCLUDES += emc/kinematics_userspace diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c new file mode 100644 index 00000000000..7d8234ce1d2 --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -0,0 +1,282 @@ +/******************************************************************** + * Description: kinematics_user.c + * Non-RT loader for kinematics modules + * + * Loads a kinematics .so with dlopen and calls the nonrt_attach() it + * exports. The module binds its own haldata to the value cells of the + * pins the running RT instance owns and hands back its unmodified + * forward and inverse entry points, so this process evaluates exactly + * the kinematics the machine is running, at whatever poses it likes. + * + * Identity kinematics needs no module code: the module says so through + * nonrt_ops_t and this file maps joints to axes directly. + * + * A module that does not export nonrt_attach() is not an error. The + * context comes back flagged rt_only and the caller does without + * kinematic limits for that machine. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "kinematics_user.h" +#include +#include +#include +#include +#include +#include + +#include "config.h" /* EMC2_HOME */ + +typedef int (*nonrt_attach_fn)(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + +/* + * Pin lookup on behalf of a dlopened kinematics module. + * + * This is the only place a name is looked up, and it runs here rather + * than inside the module because this file is userspace code linked + * against liblinuxcnchal, while the module is built as an RT object. + * + * hal_getref_p() returns the value cell of the pin, or of the signal + * when the pin is netted. The netted case is not exotic: in the + * bridgemill sim config 5axiskins.pivot-length is driven from the tool + * offset, so a bound copy has to follow the signal. + */ +static int resolve_pin(const char *pin_name, hal_type_t type, + hal_refs_u *out, void *arg) +{ + hal_query_t q; + (void)arg; + + if (!pin_name || !out) return -1; + + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + + if (hal_getref_p(&q) != 0) { + fprintf(stderr, "kinematicsUserInit: no such pin '%s'\n", pin_name); + return -1; + } + if (q.pp.type != type) { + fprintf(stderr, "kinematicsUserInit: pin '%s' has the wrong type\n", + pin_name); + return -1; + } + + *out = q.pp.ref; + return 0; +} + +struct KinematicsUserContext { + int initialized; + int rt_only; /* 1 if the module exports no nonrt_attach() */ + int is_identity; /* 1 for identity kinematics: no module code needed */ + KINEMATICS_TYPE kins_type; + void *rt_handle; /* dlopen handle */ + nonrt_ops_t ops; + int num_joints; + int joint_to_axis[KINEMATICS_USER_MAX_JOINTS]; /* identity path only */ + char module_name[64]; +}; + +/* ======================================================================== + * Identity joint mapping + * ======================================================================== */ + +static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coords) +{ + int i, j = 0; + for (i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) ctx->joint_to_axis[i] = -1; + if (!coords) return; + for (; *coords && j < ctx->num_joints; coords++) { + int axis; + switch (tolower((unsigned char)*coords)) { + case 'x': axis = 0; break; case 'y': axis = 1; break; + case 'z': axis = 2; break; case 'a': axis = 3; break; + case 'b': axis = 4; break; case 'c': axis = 5; break; + case 'u': axis = 6; break; case 'v': axis = 7; break; + case 'w': axis = 8; break; default: continue; + } + ctx->joint_to_axis[j++] = axis; + } +} + +/* ======================================================================== + * Module loading + * ======================================================================== */ + +static int load_module(KinematicsUserContext *ctx, + const char *module_name, + const char *coordinates) +{ + char module_path[512]; + void *handle; + nonrt_attach_fn attach; + + snprintf(module_path, sizeof(module_path), + "%s/rtlib/%s.so", EMC2_HOME, module_name); + + handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + if (!handle) { + fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", + module_path, dlerror()); + return -1; + } + ctx->rt_handle = handle; + + attach = (nonrt_attach_fn)dlsym(handle, "nonrt_attach"); + if (!attach) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no nonrt_attach\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (attach(coordinates, &ctx->ops, resolve_pin, NULL) != 0) { + fprintf(stderr, "kinematicsUserInit: nonrt_attach failed for '%s'\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (ctx->ops.is_identity) { + ctx->is_identity = 1; + ctx->kins_type = KINEMATICS_IDENTITY; + return 0; + } + + if (!ctx->ops.forward || !ctx->ops.inverse) { + fprintf(stderr, "kinematicsUserInit: '%s' set no fwd/inv\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + ctx->kins_type = KINEMATICS_BOTH; + return 0; +} + +/* ======================================================================== + * Public API + * ======================================================================== */ + +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates) +{ + KinematicsUserContext *ctx; + + if (!kins_type || num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS) { + fprintf(stderr, "kinematicsUserInit: invalid arguments\n"); + return NULL; + } + + ctx = (KinematicsUserContext *)calloc(1, sizeof(KinematicsUserContext)); + if (!ctx) return NULL; + + ctx->num_joints = num_joints; + strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); + + load_module(ctx, kins_type, coordinates); + + if (ctx->is_identity) { + fill_identity_joint_map(ctx, coordinates); + } + + ctx->initialized = 1; + return ctx; +} + +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints) +{ + if (!ctx || !ctx->initialized || !world || !joints) return -1; + + if (ctx->is_identity) { + int i; + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + joints[i] = (ax >= 0) ? emcPoseGetAxis(world, ax) : 0.0; + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.inverse(world, joints, NULL, NULL); +} + +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world) +{ + if (!ctx || !ctx->initialized || !joints || !world) return -1; + + if (ctx->is_identity) { + int i; + memset(world, 0, sizeof(*world)); + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + if (ax >= 0) emcPoseSetAxis(world, ax, joints[i]); + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.forward(joints, world, NULL, NULL); +} + +int kinematicsUserIsIdentity(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->is_identity; +} + +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->num_joints; +} + +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return KINEMATICS_IDENTITY; + return ctx->kins_type; +} + +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return "unknown"; + return ctx->module_name; +} + +int kinematicsUserRefreshParams(KinematicsUserContext* ctx) +{ + (void)ctx; + return 0; /* nothing to refresh: the bound pins are the live values */ +} + +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 1; + return ctx->rt_only; +} + +void kinematicsUserFree(KinematicsUserContext* ctx) +{ + if (ctx) { + if (ctx->rt_handle) dlclose(ctx->rt_handle); + free(ctx); + } +} diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h new file mode 100644 index 00000000000..256f69ee9c8 --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -0,0 +1,187 @@ +/******************************************************************** + * Description: kinematics_user.h + * Userspace kinematics interface for trajectory planning + * + * This provides a userspace-compatible kinematics interface that mirrors + * the RT kinematics interface. Used by the 9D planner to compute joint + * positions from world coordinates without requiring RT kernel calls. + * + * The RT kinematics module pushes parameters into HAL shmem each servo cycle. + * Userspace maps the shmem block read-only and calls the nonrt_ math functions + * directly, eliminating the per-call HAL pin list walk. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef KINEMATICS_USER_H +#define KINEMATICS_USER_H + +#include /* EmcPose */ +#include /* KINEMATICS_TYPE, flags */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Maximum number of joints supported */ +#define KINEMATICS_USER_MAX_JOINTS 9 + +/* Axis coordinate indices for EmcPose */ +typedef enum { + AXIS_X = 0, AXIS_Y = 1, AXIS_Z = 2, + AXIS_A = 3, AXIS_B = 4, AXIS_C = 5, + AXIS_U = 6, AXIS_V = 7, AXIS_W = 8, + AXIS_COUNT = 9 +} AxisIndex; + +/* Opaque context for userspace kinematics */ +typedef struct KinematicsUserContext KinematicsUserContext; + +/** + * Initialize userspace kinematics context + * + * @param kins_type Kinematics module name (e.g., "trivkins", "5axiskins", "maxkins") + * @param num_joints Number of joints in the machine + * @param coordinates Coordinate string (e.g., "XYZABC", "XYZBCW") + * @return Allocated context, or NULL if kinematics type not supported + */ +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates); + +/** + * Perform inverse kinematics (world coords -> joint positions) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) + * @param joints Output array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @return 0 on success, -1 on failure + */ +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints); + +/** + * Perform forward kinematics (joint positions -> world coords) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param world Output world coordinates + * @return 0 on success, -1 on failure + */ +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world); + +/** + * Check if kinematics type is identity (world coords = joint coords) + * + * @param ctx Kinematics context + * @return 1 if identity, 0 if not + */ +int kinematicsUserIsIdentity(KinematicsUserContext* ctx); + +/** + * Get number of joints + * + * @param ctx Kinematics context + * @return Number of joints + */ +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx); + +/** + * Get KINEMATICS_TYPE (IDENTITY, BOTH, FORWARD_ONLY, INVERSE_ONLY) + * + * @param ctx Kinematics context + * @return KINEMATICS_TYPE enum value + */ +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); + +/** + * Get kinematics module name + * + * @param ctx Kinematics context + * @return Module name string (e.g., "5axiskins") + */ +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); + +/** + * Refresh kinematics parameters (no-op in new shmem architecture) + * + * The RT module pushes parameters into HAL shmem every servo cycle. + * This function is kept for API compatibility but does nothing. + * + * @param ctx Kinematics context + * @return 0 always + */ +int kinematicsUserRefreshParams(KinematicsUserContext* ctx); + +/** + * Check if this context is RT-only (HAL struct not found) + * + * An RT-only module has not registered a ".params" HAL struct + * via hal_struct_newf(). Planner 2 is unavailable for such modules. + * + * @param ctx Kinematics context + * @return 1 if RT-only (planner 2 unavailable), 0 if shmem path active + */ +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx); + +/** + * Free kinematics context + * + * @param ctx Context to free + */ +void kinematicsUserFree(KinematicsUserContext* ctx); + +/** + * Get axis value from EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @return Axis value + */ +static inline double emcPoseGetAxis(const EmcPose* pose, int axis) { + switch (axis) { + case AXIS_X: return pose->tran.x; + case AXIS_Y: return pose->tran.y; + case AXIS_Z: return pose->tran.z; + case AXIS_A: return pose->a; + case AXIS_B: return pose->b; + case AXIS_C: return pose->c; + case AXIS_U: return pose->u; + case AXIS_V: return pose->v; + case AXIS_W: return pose->w; + default: return 0.0; + } +} + +/** + * Set axis value in EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @param value Value to set + */ +static inline void emcPoseSetAxis(EmcPose* pose, int axis, double value) { + switch (axis) { + case AXIS_X: pose->tran.x = value; break; + case AXIS_Y: pose->tran.y = value; break; + case AXIS_Z: pose->tran.z = value; break; + case AXIS_A: pose->a = value; break; + case AXIS_B: pose->b = value; break; + case AXIS_C: pose->c = value; break; + case AXIS_U: pose->u = value; break; + case AXIS_V: pose->v = value; break; + case AXIS_W: pose->w = value; break; + } +} + +#ifdef __cplusplus +} +#endif + +#endif /* KINEMATICS_USER_H */ diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile new file mode 100644 index 00000000000..553849e7ba5 --- /dev/null +++ b/src/emc/motion_planning/Submakefile @@ -0,0 +1,46 @@ +INCLUDES += emc/motion_planning +INCLUDES += emc/kinematics_userspace + +# Jacobian-based world-space limit calculation, plus the non-RT kinematics +# loader it sits on top of. +LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ + jacobian.cc \ + joint_limits.cc \ + ) + +LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ + kinematics_user.c \ + ) + +USERSRCS += $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS) + +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CXXSRCS)): EXTRAFLAGS = -fPIC +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CSRCS)): EXTRAFLAGS = -fPIC -D_GNU_SOURCE + +../lib/libkinslimits.so.0: $(call TOOBJS, $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS)) \ + ../lib/libposemath.so.0 ../lib/liblinuxcnchal.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../lib + $(Q)$(CXX) $(LDFLAGS) -Wl,-soname,$(notdir $@) -shared -o $@ $^ -ldl + +../lib/libkinslimits.so: ../lib/libkinslimits.so.0 + ln -sf $(notdir $<) $@ + +TARGETS += ../lib/libkinslimits.so ../lib/libkinslimits.so.0 + +# Diagnostic: print the Jacobian and the caps it implies for one move. +KINSLIMITS_SRCS := emc/motion_planning/kinslimits.cc +USERSRCS += $(KINSLIMITS_SRCS) + +../bin/kinslimits: $(call TOOBJS, $(KINSLIMITS_SRCS)) \ + ../lib/libkinslimits.so.0 ../lib/liblinuxcnchal.so.0 ../lib/libposemath.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../bin + $(Q)$(CXX) $(LDFLAGS) -o $@ $^ + +TARGETS += ../bin/kinslimits + +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh +$(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)): ../include/%.hh: emc/motion_planning/%.hh + cp $^ $@ +HEADERS += $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)) diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc new file mode 100644 index 00000000000..a7d5a7661e7 --- /dev/null +++ b/src/emc/motion_planning/jacobian.cc @@ -0,0 +1,197 @@ +/******************************************************************** + * Description: jacobian.cc + * Jacobian calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "jacobian.hh" +#include +#include +#include + +namespace motion_planning { + +JacobianCalculator::JacobianCalculator() + : kins_ctx_(nullptr), + is_identity_(false), + num_joints_(0) { +} + +JacobianCalculator::~JacobianCalculator() { + // kins_ctx_ is owned externally +} + +bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { + if (!kins_ctx) { + return false; + } + + kins_ctx_ = kins_ctx; + is_identity_ = (kinematicsUserIsIdentity(kins_ctx) != 0); + num_joints_ = kinematicsUserGetNumJoints(kins_ctx); + + return true; +} + +void JacobianCalculator::computeTrivkins(double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // For trivkins, the Jacobian is identity (with axis mapping) + // Since trivkins maps: joint[i] = world_axis[mapped_axis[i]] + // The Jacobian is: J[joint][axis] = 1 if axis == mapped_axis[joint], else 0 + + // For a simple XYZ trivkins: + // J[0][AXIS_X] = 1 (joint 0 = X) + // J[1][AXIS_Y] = 1 (joint 1 = Y) + // J[2][AXIS_Z] = 1 (joint 2 = Z) + // etc. + + // We need to query the kinematics context for the mapping. + // Since the context is opaque, we use inverse kinematics to determine + // the mapping. + + // Test each axis: perturb it and see which joint changes + EmcPose zero_pose; + ZERO_EMC_POSE(zero_pose); + double zero_joints[9]; + kinematicsUserInverse(kins_ctx_, &zero_pose, zero_joints); + + for (int axis = 0; axis < AXIS_COUNT; axis++) { + EmcPose test_pose = zero_pose; + emcPoseSetAxis(&test_pose, axis, 1.0); + + double test_joints[9]; + kinematicsUserInverse(kins_ctx_, &test_pose, test_joints); + + for (int joint = 0; joint < num_joints_; joint++) { + double delta = test_joints[joint] - zero_joints[joint]; + if (std::fabs(delta) > 0.5) { + // This axis maps to this joint + J[joint][axis] = 1.0; + } + } + } +} + +bool JacobianCalculator::computeNumerical(const EmcPose& pose, double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // Compute joints at nominal pose + double joints_center[9]; + if (kinematicsUserInverse(kins_ctx_, &pose, joints_center) != 0) { + return false; + } + + // Perturb each axis and compute derivatives + for (int axis = 0; axis < AXIS_COUNT; axis++) { + // Choose perturbation size based on axis type + double delta = (axis < 3 || axis >= 6) ? DELTA_LINEAR : DELTA_ROTARY; + + // Positive perturbation + EmcPose pose_plus = pose; + double val_plus = emcPoseGetAxis(&pose_plus, axis) + delta; + emcPoseSetAxis(&pose_plus, axis, val_plus); + + double joints_plus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_plus, joints_plus) != 0) { + // Kinematics failed - use one-sided difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Negative perturbation + EmcPose pose_minus = pose; + double val_minus = emcPoseGetAxis(&pose_minus, axis) - delta; + emcPoseSetAxis(&pose_minus, axis, val_minus); + + double joints_minus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_minus, joints_minus) != 0) { + // Use forward difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Central difference (most accurate) + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_minus[joint]) / (2.0 * delta); + } + } + + // Check for NaN/Inf values and replace with safe defaults + bool had_nan = false; + for (int joint = 0; joint < num_joints_; joint++) { + for (int axis = 0; axis < AXIS_COUNT; axis++) { + if (!std::isfinite(J[joint][axis])) { + // Replace NaN/Inf with 0 (assume no coupling) + J[joint][axis] = 0.0; + had_nan = true; + } + } + } + + // If we had NaN values, the Jacobian may be unreliable + // Return true anyway but the condition number check will catch issues + (void)had_nan; // Could log this in debug mode + + return true; +} + +bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { + if (!kins_ctx_) { + return false; + } + + if (is_identity_) { + // For trivkins, use the fast identity computation + computeTrivkins(J); + return true; + } else { + // For non-trivial kinematics, use numerical differentiation + return computeNumerical(pose, J); + } +} + +double JacobianCalculator::conditionNumber(const double J[9][9]) { + if (is_identity_) { + // Identity matrix has condition number 1 + return 1.0; + } + + // We use a simplified condition number estimate: + // Find the ratio of largest to smallest row norms + // This is not the true 2-norm condition number, but gives a rough indication + + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int joint = 0; joint < num_joints_; joint++) { + double row_norm = 0.0; + for (int axis = 0; axis < AXIS_COUNT; axis++) { + row_norm += J[joint][axis] * J[joint][axis]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + // Near-singular: a row is almost zero + return 1e18; + } + + return max_row_norm / min_row_norm; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh new file mode 100644 index 00000000000..61adca56f1f --- /dev/null +++ b/src/emc/motion_planning/jacobian.hh @@ -0,0 +1,104 @@ +/******************************************************************** + * Description: jacobian.hh + * Jacobian calculation for userspace kinematics trajectory planning + * + * Computes the Jacobian matrix relating world velocities to joint + * velocities. For trivkins this is the identity matrix. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JACOBIAN_HH +#define JACOBIAN_HH + +// emcpos.h includes posemath.h which has C++ function overloads +// so we can't use extern "C" around it +#include + +extern "C" { +#include +} + +namespace motion_planning { + +/** + * Jacobian calculator class + * + * Computes the Jacobian matrix J where: + * joint_velocities = J × world_velocities + * + * For trivkins, J is the identity matrix (with appropriate axis mapping). + * For non-trivial kinematics, J is computed via numerical differentiation. + */ +class JacobianCalculator { +public: + JacobianCalculator(); + ~JacobianCalculator(); + + /** + * Initialize with kinematics context + * + * @param kins_ctx Userspace kinematics context + * @return true on success + */ + bool init(KinematicsUserContext* kins_ctx); + + /** + * Compute Jacobian at a given pose + * + * The Jacobian J[joint][axis] relates: + * d(joint[j])/dt = sum over axis a of J[j][a] * d(axis[a])/dt + * + * @param pose World pose at which to compute Jacobian + * @param J Output 9×9 Jacobian matrix [joint][axis] + * @return true on success, false on failure + */ + bool compute(const EmcPose& pose, double J[9][9]); + + /** + * Compute condition number of Jacobian + * + * The condition number indicates how close to a singularity the pose is. + * High condition number = near singularity. + * + * For trivkins, always returns 1.0 (no singularities). + * + * @param J Jacobian matrix + * @return Condition number (≥ 1.0), or -1.0 on error + */ + double conditionNumber(const double J[9][9]); + + /** + * Check if current kinematics is identity (trivkins) + */ + bool isIdentity() const { return is_identity_; } + +private: + /** + * Compute Jacobian for trivkins (identity with axis mapping) + */ + void computeTrivkins(double J[9][9]); + + /** + * Compute Jacobian via numerical differentiation + * Uses central differences: J[j][a] = (f(x+h) - f(x-h)) / (2h) + */ + bool computeNumerical(const EmcPose& pose, double J[9][9]); + + KinematicsUserContext* kins_ctx_; + bool is_identity_; + int num_joints_; + + // Perturbation size for numerical differentiation (mm or degrees) + // Must be large enough for kinematics to produce stable results + // but small enough for accurate derivatives + static constexpr double DELTA_LINEAR = 0.1; // 0.1 mm + static constexpr double DELTA_ROTARY = 0.1; // 0.1 degrees +}; + +} // namespace motion_planning + +#endif // JACOBIAN_HH diff --git a/src/emc/motion_planning/joint_limits.cc b/src/emc/motion_planning/joint_limits.cc new file mode 100644 index 00000000000..ee4ee34d06f --- /dev/null +++ b/src/emc/motion_planning/joint_limits.cc @@ -0,0 +1,358 @@ +/******************************************************************** + * Description: joint_limits.cc + * Joint limit calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "joint_limits.hh" +#include +#include +#include + +namespace motion_planning { + +JointLimitCalculator::JointLimitCalculator() + : num_joints_(0), + initialized_(false) { +} + +JointLimitCalculator::~JointLimitCalculator() { +} + +bool JointLimitCalculator::init(int num_joints) { + if (num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS) { + return false; + } + + num_joints_ = num_joints; + + // Initialize with default (very permissive) limits + for (int i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) { + limits_[i] = JointLimitConfig(); + } + + initialized_ = true; + return true; +} + +bool JointLimitCalculator::setJointLimits(int joint, const JointLimitConfig& limits) { + if (joint < 0 || joint >= num_joints_) { + return false; + } + limits_[joint] = limits; + return true; +} + +const JointLimitConfig& JointLimitCalculator::getJointLimits(int joint) const { + static JointLimitConfig default_limits; + if (joint < 0 || joint >= num_joints_) { + return default_limits; + } + return limits_[joint]; +} + +double JointLimitCalculator::getJointVelLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].vel_limit; +} + +double JointLimitCalculator::getJointAccLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].acc_limit; +} + +double JointLimitCalculator::getJointJerkLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].jerk_limit; +} + +bool JointLimitCalculator::updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits) { + if (!initialized_) { + return false; + } + + // Update limits from arrays + // This is used to refresh limits from shared memory (motion status), + // which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + for (int j = 0; j < num_joints_; j++) { + if (vel_limits) limits_[j].vel_limit = vel_limits[j]; + if (acc_limits) limits_[j].acc_limit = acc_limits[j]; + if (min_pos) limits_[j].min_pos_limit = min_pos[j]; + if (max_pos) limits_[j].max_pos_limit = max_pos[j]; + if (jerk_limits) limits_[j].jerk_limit = jerk_limits[j]; + } + + return true; +} + +bool JointLimitCalculator::checkPositionLimits(const double joint_pos[9]) { + for (int j = 0; j < num_joints_; j++) { + if (joint_pos[j] > limits_[j].max_pos_limit || + joint_pos[j] < limits_[j].min_pos_limit) { + return false; + } + } + return true; +} + +double JointLimitCalculator::computeMaxVelocity(const double J[9][9], int& limiting_joint) { + // Conservative estimate: assume worst-case direction + // For each joint j, find the maximum Jacobian element magnitude + // max_world_vel = min over j of: vel_limit[j] / max(|J[j][:]|) + + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Find maximum absolute value in this row of J + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + // This joint contributes to motion + double vel_limit_world = limits_[j].vel_limit / max_abs_J; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + // Apply sanity bounds + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAcceleration(const double J[9][9], int& limiting_joint) { + // Same approach as velocity + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / max_abs_J; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerk(const double J[9][9], int& limiting_joint) { + // Same approach as velocity and acceleration + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / max_abs_J; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + + return max_world_jerk; +} + +double JointLimitCalculator::computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Compute sum(|J[j][a]| * |tangent[a]|) — the actual amplification + // for this joint along the given path direction + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double vel_limit_world = limits_[j].vel_limit / amplification; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / amplification; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / amplification; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + return max_world_jerk; +} + +bool JointLimitCalculator::computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + result.position_ok = checkPositionLimits(joint_pos); + result.condition_number = computeConditionNumber(J); + + result.max_world_vel = computeMaxVelocityForTangent(J, tangent, result.limiting_joint_vel); + result.max_world_acc = computeMaxAccelerationForTangent(J, tangent, result.limiting_joint_acc); + result.max_world_jerk = computeMaxJerkForTangent(J, tangent, result.limiting_joint_jerk); + + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +double JointLimitCalculator::computeConditionNumber(const double J[9][9]) { + // Simplified condition number: ratio of max to min row norms + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int j = 0; j < num_joints_; j++) { + double row_norm = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + row_norm += J[j][a] * J[j][a]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + return 1e18; // Near-singular + } + + return max_row_norm / min_row_norm; +} + +bool JointLimitCalculator::compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + // Check position limits + result.position_ok = checkPositionLimits(joint_pos); + + // Compute condition number + result.condition_number = computeConditionNumber(J); + + // Compute max velocity + result.max_world_vel = computeMaxVelocity(J, result.limiting_joint_vel); + + // Compute max acceleration + result.max_world_acc = computeMaxAcceleration(J, result.limiting_joint_acc); + + // Compute max jerk + result.max_world_jerk = computeMaxJerk(J, result.limiting_joint_jerk); + + // Apply singularity slowdown + // If condition number exceeds threshold, reduce limits proportionally + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/joint_limits.hh b/src/emc/motion_planning/joint_limits.hh new file mode 100644 index 00000000000..a7785ddaea4 --- /dev/null +++ b/src/emc/motion_planning/joint_limits.hh @@ -0,0 +1,238 @@ +/******************************************************************** + * Description: joint_limits.hh + * Joint limit calculation for userspace kinematics trajectory planning + * + * Uses the Jacobian to compute maximum world-space velocity and + * acceleration that respects all joint limits. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JOINT_LIMITS_HH +#define JOINT_LIMITS_HH + +// emcpos.h includes posemath.h which has C++ function overloads +#include + +extern "C" { +#include +} + +namespace motion_planning { + +/** + * Joint limit configuration + * Mirrors emcmot_joint_t limits from motion.h + */ +struct JointLimitConfig { + double max_pos_limit; // Upper soft limit on joint position + double min_pos_limit; // Lower soft limit on joint position + double vel_limit; // Maximum joint velocity + double acc_limit; // Maximum joint acceleration + double jerk_limit; // Maximum joint jerk (for S-curve planning) + + JointLimitConfig() : + max_pos_limit(1e9), + min_pos_limit(-1e9), + vel_limit(1e9), + acc_limit(1e9), + jerk_limit(1e9) {} +}; + +/** + * Result of joint limit calculation + */ +struct JointLimitResult { + double max_world_vel; // Max world velocity respecting joint vel limits + double max_world_acc; // Max world accel respecting joint acc limits + double max_world_jerk; // Max world jerk (for S-curve planning) + bool position_ok; // True if joint positions are within soft limits + int limiting_joint_vel; // Joint index that limits velocity (-1 if none) + int limiting_joint_acc; // Joint index that limits acceleration + int limiting_joint_jerk; // Joint index that limits jerk + double condition_number; // Jacobian condition number (singularity indicator) + + JointLimitResult() : + max_world_vel(1e9), + max_world_acc(1e9), + max_world_jerk(1e9), + position_ok(true), + limiting_joint_vel(-1), + limiting_joint_acc(-1), + limiting_joint_jerk(-1), + condition_number(1.0) {} +}; + +/** + * Joint limit calculator class + * + * Computes maximum world-space velocity/acceleration that respects + * all joint limits, given the Jacobian at a pose. + * + * The relationship is: + * joint_vel = J × world_vel + * |joint_vel[j]| ≤ joint_limit[j].vel_limit for all j + * + * To find max world velocity, we solve: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / |J[j] · direction| + * + * For a general direction, we use a conservative estimate: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / max(|J[j][:]|) + */ +class JointLimitCalculator { +public: + JointLimitCalculator(); + ~JointLimitCalculator(); + + /** + * Initialize with number of joints + * + * @param num_joints Number of joints + * @return true on success + */ + bool init(int num_joints); + + /** + * Set limits for a joint + * + * @param joint Joint index (0 to num_joints-1) + * @param limits Limit configuration for this joint + * @return true on success + */ + bool setJointLimits(int joint, const JointLimitConfig& limits); + + /** + * Update limits for all joints at once + * + * This is used to refresh limits from shared memory (motion status structure), + * which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + * + * @param vel_limits Array of velocity limits [num_joints] + * @param acc_limits Array of acceleration limits [num_joints] + * @param min_pos Array of min position limits [num_joints] + * @param max_pos Array of max position limits [num_joints] + * @param jerk_limits Array of jerk limits [num_joints] (can be NULL) + * @return true on success + */ + bool updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits = nullptr); + + /** + * Get limits for a joint + */ + const JointLimitConfig& getJointLimits(int joint) const; + + /** + * Get velocity limit for a specific joint + */ + double getJointVelLimit(int joint) const; + + /** + * Get acceleration limit for a specific joint + */ + double getJointAccLimit(int joint) const; + + /** + * Get jerk limit for a specific joint + */ + double getJointJerkLimit(int joint) const; + + /** + * Compute world-space limits at a pose given the Jacobian + * + * Uses conservative direction-independent bound (max |J[j][:]|). + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Compute world-space limits for a specific path tangent direction + * + * Uses the actual path tangent to compute tight bounds. The tangent + * is in world-axis units per unit of the Ruckig path parameter (which + * may be XYZ arc length). Rotary components can be >> 1.0 when + * rotary axes move much more than linear axes per unit path. + * + * The bound for each joint is: + * limit[j] / sum(|J[j][a]| * |tangent[a]|) + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param tangent Path tangent: d(world_axis)/d(path_param) [9] + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Check if joint positions are within soft limits + * + * @param joint_pos Array of joint positions + * @return true if all joints within limits + */ + bool checkPositionLimits(const double joint_pos[9]); + + /** + * Get the number of joints + */ + int getNumJoints() const { return num_joints_; } + +private: + /** + * Compute maximum world velocity from joint velocity limits and Jacobian + * + * Uses conservative estimate: max over all directions + */ + double computeMaxVelocity(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world acceleration from joint accel limits and Jacobian + */ + double computeMaxAcceleration(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world jerk from joint jerk limits and Jacobian + */ + double computeMaxJerk(const double J[9][9], int& limiting_joint); + + /** + * Tangent-aware versions: use sum(|J[j][a]| * |tangent[a]|) instead of max(|J[j][a]|) + */ + double computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + + /** + * Compute Jacobian condition number (simplified) + */ + double computeConditionNumber(const double J[9][9]); + + int num_joints_; + JointLimitConfig limits_[KINEMATICS_USER_MAX_JOINTS]; + bool initialized_; +}; + +} // namespace motion_planning + +#endif // JOINT_LIMITS_HH diff --git a/src/emc/motion_planning/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc new file mode 100644 index 00000000000..258d621d03a --- /dev/null +++ b/src/emc/motion_planning/kinslimits.cc @@ -0,0 +1,268 @@ +/******************************************************************** + * Description: kinslimits.cc + * Diagnostic tool: print the Jacobian and the world-space velocity, + * acceleration and jerk caps that a given kinematics module imposes + * on a straight move between two poses. + * + * The tool attaches to a running HAL instance, loads the kinematics + * module through the non-RT interface, samples the move, and reports + * the most restrictive cap found along it. The sampling loop here is + * the same one the trajectory planner uses to cap a segment. + * + * Example (in a terminal with a running config, or under halrun): + * + * halrun -I + * halcmd: loadrt 5axiskins coordinates=XYZBCW + * halcmd: setp 5axiskins.pivot-length 100 + * halcmd: loadusr -w kinslimits --module 5axiskins --joints 6 \ + * --coords XYZBCW --start 0,0,0,0,0,0,0,0,0 \ + * --end 100,0,0,0,90,0,0,0,0 \ + * --vel 100,100,100,30,30,30 --acc 500,500,500,200,200,200 + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include "jacobian.hh" +#include "joint_limits.hh" + +using namespace motion_planning; + +static const char *AXIS_NAME[9] = {"X","Y","Z","A","B","C","U","V","W"}; + +static std::vector parse_list(const char *s) +{ + std::vector out; + const char *p = s; + while (*p) { + char *endp = nullptr; + double v = strtod(p, &endp); + if (endp == p) break; + out.push_back(v); + p = endp; + while (*p == ',' || *p == ' ') p++; + } + return out; +} + +static void list_to_pose(const std::vector& v, EmcPose *p) +{ + double a[9] = {0,0,0,0,0,0,0,0,0}; + for (size_t i = 0; i < v.size() && i < 9; i++) a[i] = v[i]; + p->tran.x = a[0]; p->tran.y = a[1]; p->tran.z = a[2]; + p->a = a[3]; p->b = a[4]; p->c = a[5]; + p->u = a[6]; p->v = a[7]; p->w = a[8]; +} + +static double pose_axis(const EmcPose& p, int ax) +{ + switch (ax) { + case 0: return p.tran.x; case 1: return p.tran.y; case 2: return p.tran.z; + case 3: return p.a; case 4: return p.b; case 5: return p.c; + case 6: return p.u; case 7: return p.v; default: return p.w; + } +} + +static void set_pose_axis(EmcPose *p, int ax, double val) +{ + switch (ax) { + case 0: p->tran.x = val; break; case 1: p->tran.y = val; break; + case 2: p->tran.z = val; break; case 3: p->a = val; break; + case 4: p->b = val; break; case 5: p->c = val; break; + case 6: p->u = val; break; case 7: p->v = val; break; + default: p->w = val; break; + } +} + +static void usage(const char *argv0) +{ + fprintf(stderr, + "usage: %s --module NAME --joints N --coords LETTERS\n" + " --start x,y,z,a,b,c,u,v,w --end x,y,z,a,b,c,u,v,w\n" + " --vel v0,v1,... --acc a0,a1,... [--jerk j0,j1,...]\n" + " [--samples N] [--singularity COND]\n" + "\n" + "Prints the Jacobian and the world-space caps the joint limits imply\n" + "for a straight move from --start to --end. Requires a running HAL\n" + "instance with the kinematics module loaded.\n", argv0); +} + +int main(int argc, char **argv) +{ + const char *module = nullptr; + const char *coords = nullptr; + int num_joints = 0; + int samples = 11; + double singularity = 100.0; + std::vector start_v, end_v, vel_v, acc_v, jerk_v; + + for (int i = 1; i < argc; i++) { + const char *a = argv[i]; + const char *next = (i + 1 < argc) ? argv[i + 1] : nullptr; + if (!strcmp(a, "--module") && next) { module = next; i++; } + else if (!strcmp(a, "--coords") && next) { coords = next; i++; } + else if (!strcmp(a, "--joints") && next) { num_joints = atoi(next); i++; } + else if (!strcmp(a, "--samples") && next) { samples = atoi(next); i++; } + else if (!strcmp(a, "--singularity") && next){ singularity = atof(next); i++; } + else if (!strcmp(a, "--start") && next) { start_v = parse_list(next); i++; } + else if (!strcmp(a, "--end") && next) { end_v = parse_list(next); i++; } + else if (!strcmp(a, "--vel") && next) { vel_v = parse_list(next); i++; } + else if (!strcmp(a, "--acc") && next) { acc_v = parse_list(next); i++; } + else if (!strcmp(a, "--jerk") && next) { jerk_v = parse_list(next); i++; } + else { usage(argv[0]); return 1; } + } + + if (!module || !coords || num_joints < 1 || + start_v.empty() || end_v.empty() || vel_v.empty() || acc_v.empty()) { + usage(argv[0]); + return 1; + } + if ((int)vel_v.size() < num_joints || (int)acc_v.size() < num_joints) { + fprintf(stderr, "kinslimits: --vel and --acc need %d entries\n", num_joints); + return 1; + } + if (samples < 2) samples = 2; + + int comp_id = hal_init("kinslimits"); + if (comp_id < 0) { + fprintf(stderr, "kinslimits: hal_init failed (is HAL running?)\n"); + return 1; + } + + KinematicsUserContext *ctx = kinematicsUserInit(module, num_joints, coords); + if (!ctx) { + fprintf(stderr, "kinslimits: kinematicsUserInit failed for '%s'\n", module); + hal_exit(comp_id); + return 1; + } + if (kinematicsUserIsRtOnly(ctx)) { + fprintf(stderr, "kinslimits: '%s' is RT-only, no non-RT interface\n", module); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + JacobianCalculator jac; + JointLimitCalculator lim; + if (!jac.init(ctx) || !lim.init(num_joints)) { + fprintf(stderr, "kinslimits: calculator init failed\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + std::vector minpos(num_joints, -1e9), maxpos(num_joints, 1e9); + if ((int)jerk_v.size() < num_joints) jerk_v.assign(num_joints, 1e9); + lim.updateAllLimits(vel_v.data(), acc_v.data(), + minpos.data(), maxpos.data(), jerk_v.data()); + + EmcPose start, end; + list_to_pose(start_v, &start); + list_to_pose(end_v, &end); + + /* Path parameter: XYZ arc length, falling back to the largest rotary + delta for a pure rotary move, matching what the planner uses. */ + double dx = end.tran.x - start.tran.x; + double dy = end.tran.y - start.tran.y; + double dz = end.tran.z - start.tran.z; + double target = sqrt(dx*dx + dy*dy + dz*dz); + if (target < 1e-12) { + for (int ax = 3; ax < 9; ax++) { + double d = fabs(pose_axis(end, ax) - pose_axis(start, ax)); + if (d > target) target = d; + } + } + if (target < 1e-12) { + fprintf(stderr, "kinslimits: start and end are the same pose\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + /* tangent[a] = d(world axis a) / d(path parameter) */ + double tangent[9]; + for (int ax = 0; ax < 9; ax++) { + tangent[ax] = (pose_axis(end, ax) - pose_axis(start, ax)) / target; + } + + printf("module : %s (%s, %d joints)%s\n", module, coords, num_joints, + kinematicsUserIsIdentity(ctx) ? " [identity]" : ""); + printf("path length : %.6f (tangent units per path unit)\n", target); + printf("tangent :"); + for (int ax = 0; ax < 9; ax++) { + if (fabs(tangent[ax]) > 1e-12) printf(" %s=%.4f", AXIS_NAME[ax], tangent[ax]); + } + printf("\n\n"); + + double min_vel = 1e9, min_acc = 1e9, min_jerk = 1e9, max_cond = 1.0; + int at_vel = -1, at_acc = -1, at_jerk = -1; + double min_vel_s = 0.0; + + for (int i = 0; i < samples; i++) { + double frac = (double)i / (double)(samples - 1); + EmcPose p; + for (int ax = 0; ax < 9; ax++) { + set_pose_axis(&p, ax, + pose_axis(start, ax) + frac * (pose_axis(end, ax) - pose_axis(start, ax))); + } + + double joints[KINEMATICS_USER_MAX_JOINTS] = {0}; + if (kinematicsUserInverse(ctx, &p, joints) != 0) { + printf("sample %2d: inverse kinematics failed\n", i); + continue; + } + + double J[9][9]; + if (!jac.compute(p, J)) { + printf("sample %2d: Jacobian failed\n", i); + continue; + } + + double jpad[9] = {0}; + for (int j = 0; j < num_joints && j < 9; j++) jpad[j] = joints[j]; + + JointLimitResult r; + if (!lim.computeForTangent(J, jpad, tangent, r, singularity)) { + printf("sample %2d: limit calculation failed\n", i); + continue; + } + + printf("s=%.3f vel<=%10.3f (j%d) acc<=%10.1f (j%d) jerk<=%12.1f (j%d) cond=%.2f\n", + frac, r.max_world_vel, r.limiting_joint_vel, + r.max_world_acc, r.limiting_joint_acc, + r.max_world_jerk, r.limiting_joint_jerk, r.condition_number); + + if (r.max_world_vel < min_vel) { min_vel = r.max_world_vel; at_vel = r.limiting_joint_vel; min_vel_s = frac; } + if (r.max_world_acc < min_acc) { min_acc = r.max_world_acc; at_acc = r.limiting_joint_acc; } + if (r.max_world_jerk < min_jerk) { min_jerk = r.max_world_jerk; at_jerk = r.limiting_joint_jerk; } + if (r.condition_number > max_cond) max_cond = r.condition_number; + + if (i == 0) { + printf(" Jacobian at start (rows = joints, cols = XYZABCUVW):\n"); + for (int j = 0; j < num_joints && j < 9; j++) { + printf(" j%d:", j); + for (int ax = 0; ax < 9; ax++) printf(" %8.4f", J[j][ax]); + printf("\n"); + } + } + } + + printf("\nsegment cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), jerk %.1f (joint %d)\n", + min_vel, at_vel, min_vel_s, min_acc, at_acc, min_jerk, at_jerk); + printf("worst cond : %.3f\n", max_cond); + + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 0; +} From 51dfa51127a4e309b5de93b06bb370f4582f75dc Mon Sep 17 00:00:00 2001 From: david mueller Date: Sun, 23 Aug 2026 13:32:45 +1000 Subject: [PATCH 18/20] twp: split the machine maths out of the tilted work plane remap The remap carried the geometry of every supported machine inside itself, as branches on the (primary, secondary) joint letter pair in kins_calc_secondary, kins_calc_primary, kins_tool_transformation and kins_calc_tool_rot_c_for_horizontal_x. Adding a machine meant adding a branch to each, and a machine whose maths did not fit that shape could not be added at all. The generic half now lives in remap.py and the machine half in a remap_funcs_twp.py beside each config, pulled in with a plain import. Eleven functions form the interface, the ones the generic side needs to ask a machine: which joint angles reach a tool orientation, how to build the transformation matrix, what the default tool-x direction is, what to write on the module pins. The two configs here supply their own, so the (C,B) and (C,A) branches that were interleaved in one file are now one file each. The generic remap.py and the machine files are David Mueller's, from https://github.com/Sigma1912/LinuxCNC_Demo_Configs/tree/main/5axis-twp, where this separation was worked out. His snrtr modules carry exactly the two branches this config pair needs. Adapted here: the ini is read through linuxcnc.ini rather than configparser, the kinematics switch is G12.1 rather than a write to the deprecated motion.switchkins-type pin, kins_set_values converts the two joint angles to degrees because the modules in tree read those pins in degrees, and the pins keep their existing names, so neither module changes and no config has to be rewired. Three behaviour changes come with it, all of them the machine doing what was asked where it previously did not. kins_calc_primary appended its result outside the loop over candidate secondary angles, so only the last candidate ever contributed a primary angle and the solution set was half the size it should be. With the full set, G53.1 P1 and P2 find the positive-only and negative-only solutions they were asking for instead of failing, and P0 sometimes picks a shorter move: on xyzacb-trsrn one of the test orientations is now reached with the primary at -73.87 degrees rather than 130.25, the same tool vector to nine decimal places. Candidate angles were compared in radians against limits read in degrees, so the limit test was meaningless for anything outside plus or minus 57 degrees. On xyzbca-trsrn, G53.6, G68.3 and one G53.3 case reported success while leaving the kinematics in identity with the module pins holding values from whatever ran before. They now activate the tilted work plane. Verified by driving both configs through eight orientations under G53.1 P0, P1 and P2, G53.3, G53.6 and G68.3, and comparing against the same run before the change. Where a different joint solution is chosen the resulting tool vector is identical to within 1e-9. No case fails that used to work. --- .../python/remap.py | 1476 ++++++++--------- .../xyzacb-trsrn_twp/remap_funcs_twp.py | 347 ++++ .../xyzacb-trsrn_twp/xyzacb-trsrn.ini | 2 +- .../xyzbca-trsrn_twp/remap_funcs_twp.py | 350 ++++ .../xyzbca-trsrn_twp/xyzbca-trsrn.ini | 2 +- 5 files changed, 1386 insertions(+), 791 deletions(-) create mode 100644 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py create mode 100644 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py index f4f9506a846..05fc53261a1 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py @@ -1,7 +1,7 @@ # This is a python remap for LinuxCNC implementing 'Tilted Work Plane' # G68.2, G68.3, G68.4 and related Gcodes G53.1, G53.3, G53.6, G69 # -# Copyright ()c) 2023 David Mueller +# Copyright ()c) 2025 David Mueller # # 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 @@ -13,7 +13,22 @@ # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # -# +''' +The remap does the following: + +- Parses the G68.[2,4] gcodes and constructs the requested tool orientation vectors (x,z). +- Writes and reads hal pins created and updated by'twp-helper-comp.py' (mostly for updating the gui). +- Parses the G53.[1,3,6] and uses the functions in 'remap_funcs_twp.py' to calculate all rotary joint position that result in the correct tool orientation (there may be more than just one). +- Selects the appropriate rotary angles that will respect rotary limits set in the ini file and also follow any orientation strategy requested by the operator using the 'P' word. +- Sets the kinematic modes +- Calculates new work offset values so the WCS origin after switching to TWP mode is in the requested physical position. +- Used MDI commands to: + - Move the rotary joints to the calculated positions + - Switch the WCS system to 'G59' and set the values of G59, G59.[1,2.3] to the calculated coordinates +- Parses the G69 gcodes, resets the relevant parameters and switches back to Identity kinematic mode +''' + + import sys import traceback import numpy as np @@ -23,7 +38,6 @@ from util import lineno, call_pydevd import hal - # logging import logging # this name will be printed first on each log message @@ -33,22 +47,41 @@ formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') handler.setFormatter(formatter) log.addHandler(handler) -# Manually force the log level for this module -log.setLevel(logging.ERROR) # One of DEBUG, INFO, WARNING, ERROR, CRITICAL - # set up parsing of the inifile import os import linuxcnc # get the path for the ini file used to start this config inifile = os.environ.get("INI_FILE_NAME") + +# adding the remap_funcs folder to the system path. The machine specific +# functions live beside the ini file, which is the working directory, and the +# parent is searched too so a config may keep them one level up and share them +# between variants. +cwd = os.getcwd() +parent = os.path.abspath(os.path.join(cwd, os.pardir)) +sys.path.insert(0, parent) +sys.path.insert(0, cwd) +from remap_funcs_twp import * + # instantiate the LinuxCNC ini-parser config = linuxcnc.ini(inifile) -## SPINDLE ROTARY JOINT LETTERS -# spindle primary joint +# debug setting +try: + debug_setting = config.getint('TWP', 'LOG_LEVEL', fallback=1) + if debug_setting > 4: debug_setting = 4 + if debug_setting < 0: debug_setting = 0 +except Exception as error: + debug_setting = 1 + log.warning("Unable to parse debug setting given in INI. Setting it to 1.") +debug_levels = (logging.CRITICAL, logging.ERROR, logging.WARNING, logging.INFO, logging.DEBUG) +log.setLevel(debug_levels[debug_setting]) + +## ROTARY JOINT LETTERS +# primary rotary joint (independent of the secondary joint) joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() -# spindle secondary joint (ie the one closer to the tool) +# secondary rotary joint (dependent on the primary joint) joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() if not joint_letter_primary in ('A','B','C') or not joint_letter_secondary in ('A','B','C'): @@ -58,32 +91,28 @@ else: # get the MIN/MAX limits of the respective rotary joint letters category = 'AXIS_' + joint_letter_primary - primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) - primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) - log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', joint_letter_primary, primary_min_limit, primary_max_limit) + primary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) + primary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) + log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', + joint_letter_primary, degrees(primary_min_limit), degrees(primary_max_limit)) category = 'AXIS_' + joint_letter_secondary - secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) - secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) - log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', joint_letter_secondary, secondary_min_limit, secondary_max_limit) - - -## CONNECTIONS TO THE KINEMATIC COMPONENT -# get the name of the kinematic component -kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") -# name of the hal pin that represents the nutation-angle -kins_nutation_angle = kins_comp + '_kins.nut-angle' -# name of the hal pin that represents the pre-rotation -kins_pre_rotation = kins_comp + '_kins.pre-rot' -# name of the hal pin that represents the primary joint orientation angle -kins_primary_rotation = kins_comp + '_kins.primary-angle' -# name of the hal pin that represents the secondary joint orientation angle -kins_secondary_rotation = kins_comp + '_kins.secondary-angle' + secondary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) + secondary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) + log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', + joint_letter_secondary, degrees(secondary_min_limit), degrees(secondary_max_limit)) + ## CONNECTIONS TO THE HELPER COMPONENT twp_comp = 'twp-helper-comp.' twp_is_defined = twp_comp + 'twp-is-defined' twp_is_active = twp_comp + 'twp-is-active' +# Which rotary joint should be prioritized when calculating optimal joint rotation angles +try: + optimization_priority = config.getint('TWP', 'PRIORITY', fallback=1) +except Exception as error: + log.warning("Unable to parse orientation priority given in INI. Setting it to 1.") + optimization_priority = 1 # raise InterpreterException if execute() or read() fail throw_exceptions = 1 @@ -93,7 +122,7 @@ twp_matrix = np.asmatrix(np.identity(4)) # some g68.2 p-word modes require several calls to enter all the required parameters so we -# need a flag that indicates when the twp has been defined and is ready for g53.x +# need a flag that indicates when the twp has been defined and is ready for G53.n # [current p-word, number of calls required, (state of calls required for that p mode added by g68.2)] # note that we use string since boolean True == 1, which gives wrong results if we want # to count the elements that are True because it is counted as integer '1' @@ -105,592 +134,391 @@ current_work_offset_number = 1 saved_work_offset = [0,0,0] # orientation mode refers to the strategy used to choose from the different rotary angles for a given -# tool-z vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being +# z-vector vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being # the default. (0=shortest_path , 1=positive_rotation only, 2=negative_rotation only, ) orient_mode = 0 -# defines the kinematic model for (world <-> tool) coordinates of the machine at hand -# returns 4x4 transformation matrix for given angles and 4x4 input matrix -# NOTE: these matrices must be the same as the ones used to derive the kinematic model -def kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction='fwd'): - global joint_letter_primary, joint_letter_secondary - global kins_nutation_angle - T_in = matrix_in - - ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y - Stc = sin(pre_rot) - Ctc = cos(pre_rot) - Rtc=np.matrix([[ Ctc, -Stc, 0, 0], - [ Stc, Ctc, 0, 0], - [ 0 , 0 , 1, 0], - [ 0, 0 , 0, 1]]) - - ## Define 4x4 transformation for the primary joint - # get the basic 3x3 rotation matrix (returns array) - if joint_letter_primary == 'A': - Rp = Rx(theta_1) - elif joint_letter_primary == 'B': - Rp = Ry(theta_1) - elif joint_letter_primary == 'C': - Rp = Rz(theta_1) - # add fourth column on the right - Rp = np.hstack((Rp, [[0],[0],[0]])) - # expand to 4x4 array and make into a matrix - row_4 = [0,0,0,1] - Rp = np.vstack((Rp, row_4)) - Rp = np.asmatrix(Rp) - - ## Define 4x4 transformation matrix for the secondary joint - # get the basic 3x3 rotation matrix (returns array) - if joint_letter_secondary == 'A': - Rs = Rx(theta_2) - elif joint_letter_secondary == 'B': - Rs = Ry(theta_2) - elif joint_letter_secondary == 'C': - Rs = Rz(theta_2) - # add fourth column on the right - Rs = np.hstack((Rs, [[0],[0],[0]])) - # expand to 4x4 array and make into a matrix - row_4 = [0,0,0,1] - Rs = np.vstack((Rs, row_4)) - Rs = np.asmatrix(Rs) - - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # Additional definitions for nutating joint - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - Ss = sin(theta_2) - Cs = cos(theta_2) - r = Cs + Sv*Sv*(1-Cs) - s = Cs + Cv*Cv*(1-Cs) - t = Sv*Cv*(1-Cs) - # define rotation matrix for the secondary spindle joint - Rs=np.matrix([[ Cs, -Cv*Ss, Sv*Ss, 0], - [ Cv*Ss, r, t, 0], - [ -Sv*Ss, t, s, 0], - [ 0, 0, 0, 1]]) - - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # Additional definitions for nutating joint - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - Ss = sin(theta_2) - Cs = cos(theta_2) - r = Cs + Sv*Sv*(1-Cs) - s = Cs + Cv*Cv*(1-Cs) - t = Sv*Cv*(1-Cs) - # define rotation matrix for the secondary spindle joint - Rs=np.matrix([[ r, -Cv*Ss, t, 0], - [ Cv*Ss, Cs, -Sv*Ss, 0], - [ t, Sv*Ss, s, 0], - [ 0, 0, 0, 1]]) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) - - # calculate the transformation matrix for the forward tool kinematic - matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in - # calculate the transformation matrix for the inverse tool kinematic - matrix_tool_inv = Rp*Rs*Rtc*T_in - if direction == 'fwd': - #log.debug("matrix tool fwd: \n", matrix_tool_fwd) - #log.debug("inv would have been: \n", matrix_tool_inv) - return matrix_tool_fwd - elif direction == 'inv': - #log.debug("matrix tool inv: \n", matrix_tool_inv) - #log.debug("fwd would have been: \n", matrix_tool_fwd) - return matrix_tool_inv - else: - return 0 +# define the basic rotation matrices +def Rx(th): + return np.array([[1, 0 , 0 ], + [0, cos(th), -sin(th)], + [0, sin(th), cos(th)]]) +def Ry(th): + return np.array([[ cos(th), 0, sin(th)], + [ 0 , 1, 0 ], + [-sin(th), 0, cos(th)]]) -# returns angle 'tc' required to rotate the x-axis of the tool-coords parallelto the machine-xy plane -# for given machine joint position angles. -# For G68.3 this is the default tool-x direction -# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic -def kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2): - global joint_letter_primary, joint_letter_secondary - # The idea is that the tool-x vector is parallel to the machine xy-plane when the - # z component of the x-direction vector is equal to zero - # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation - # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. - # this makes the x orientation of the tool coords horizontal and the user can set the - # rotation from there using g68.3 r - global kins_nutation_angle - v = radians(hal.get_value(kins_nutation_angle)) - Cv = cos(v) - Sv = sin(v) - Cs = cos(theta_2) - Ss = sin(theta_2) - Cp = cos(theta_1) - Sp = sin(theta_1) - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - t = Sv*Cv*(1-Cs) - tc = atan2((Sv*Ss),t) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - t = Sv*Cv*(1-Cs) - tc = atan2(-t,(Sv*Ss)) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) - # note: tool-c rotation is done using a halpin that feeds into the kinematic component and the - # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) - return tc +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) -# calculates the secondary joint position for a given tool-vector -# secondary being the joint closest to the tool -# Note: this uses functions derived from the custom kinematic -def kins_calc_secondary(self, tool_z_req): - global joint_letter_primary, joint_letter_secondary - global secondary_min_limit, secondary_max_limit - global kins_nutation_angle - epsilon = 0.000001 - theta_2_list=[] - (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) - - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) - # since we are using acos() we really have two solutions theta_2 and -theta_2 - for theta in [theta_2, -theta_2]: - log.debug('Checking if result %s is within secondary joint limits of %s and %s.', - degrees(theta), secondary_min_limit, secondary_max_limit) - if theta > secondary_min_limit and theta < secondary_max_limit: - log.debug('Adding %s to valid angles list.', degrees(theta)) - theta_2_list.append(theta) - log.debug('List of possible secondary angles: %s\n', theta_2_list) - return theta_2_list - - -# calculates the primary joint position for a given tool-vector -# Note: this uses functions derived from the custom kinematic -def kins_calc_primary(self, tool_z_req, theta_2_list): - global joint_letter_primary, joint_letter_secondary - global primary_min_limit, primary_max_limit - global kins_nutation_angle - epsilon = 0.000001 - theta_1_list=[] - (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - for i in range(len(theta_2_list)): - theta_2 = theta_2_list[i] - Ss = sin(theta_2) - Cs = cos(theta_2) - t = Sv*Cv*(1-Cs) - p = Sv * Ss - - theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - for i in range(len(theta_2_list)): - theta_2 = theta_2_list[i] - Ss = sin(theta_2) - Cs = cos(theta_2) - t = Sv*Cv*(1-Cs) - p = Sv * Ss - q = (t*Kzy - p*Kzx)/(t*t + p*p) - theta_1 = asin(q) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) - # since we are using asin() we really have two solutions theta_1 and pi-theta_2 - for theta in [theta_1, transform_to_pipi(pi - theta_1)]: - log.debug('Checking if result %s is within secondary joint limits of %s and %s.', - degrees(theta), secondary_min_limit, secondary_max_limit) - if theta > secondary_min_limit and theta < secondary_max_limit: - log.debug('Adding %s to valid angles list.', degrees(theta)) - theta_1_list.append(theta) - log.debug('List of possible secondary angles: %s\n', theta_2_list) - return theta_1_list - - -# this is from 'mika-s.github.io' -# transforms a given angle to the interval of [-pi,pi] -def transform_to_pipi(input_angle): - revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) - p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) - p2 = (np.sign(np.sign(input_angle) - + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi - output_angle = p1 - p2 - return output_angle - - -# this is from 'mika-s.github.io' -# used by 'transform_to_pipi()' -def truncated_remainder(dividend, divisor): - divided_number = dividend / divisor - divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) - remainder = dividend - divisor * divided_number - return remainder - - -# returns a list of valid primary/secondary spindle joint positions for a given tool-orientation vector -# or 'None','None' if no valid position could be found -def kins_calc_jnt_angles(self, tool_z_req): - log.debug('tool_z_requested: %s', tool_z_req) + +def calc_euler_rot_matrix(th1, th2, th3, order): # expects radians + # returns the rotation matrices for given order and angles + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + debug_msg = (f' Euler order {order} requested with angles: ' + f'{degrees(th1):.4f}, {degrees(th2):.4f}, {degrees(th3):.4f}') + log.debug(debug_msg) + if order == '131': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) + elif order=='121': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) + elif order=='212': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) + elif order=='232': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) + elif order=='323': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) + elif order=='313': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) + elif order=='123': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) + elif order=='132': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) + elif order=='213': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) + elif order=='231': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) + elif order=='321': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) + elif order=='312': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) + #log.debug(' Returning euler rotation as matrix: \n %s', matrix) + return matrix + + +def calc_joint_angles(z_vector_req, x_vector_req): + # returns a list of valid primary/secondary rotary joint positions in radians for a given orientation vector + # returns an empty list if no valid position could be found + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + log.debug(' z_vector_requested: %s', z_vector_req) + log.debug(' x_vector_requested: %s', x_vector_req) # set the tolerance value epsilon = 0.0001 # create np.array so we can easily calculate differences and check elements - tool_z_req = np.array([tool_z_req[0], tool_z_req[1], tool_z_req[2]]) - # calculate secondary joint values using kinematic specific formula - theta_2_pair = kins_calc_secondary(self, tool_z_req) - # calculate primary joint values using kinematic specific formula - theta_1_pair = kins_calc_primary(self, tool_z_req, theta_2_pair) + z_vector_req = np.array([z_vector_req[0], z_vector_req[1], z_vector_req[2]]) + # calculate joint values using kinematic specific formula + try: + (theta_1_calcd, theta_2_calcd) = kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req) + except Exception as error: + log.error('Remap_funcs: kins_calc_possible_joint_angles failure, %s', error) + + # remove any duplicate values from the results + theta_1_calcd = tuple(set(theta_1_calcd)) + theta_2_calcd = tuple(set(theta_2_calcd)) + log.debug(' Got possible angles theta_1: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_1_calcd)) + log.debug(' Got possible angles theta_2: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_2_calcd)) + if theta_1_calcd == None or theta_2_calcd == None: + return [] + angle_pairs_list = [] + # create a list of paired combinations of returned angles (theta_1 , theta_2) + for i in range(len(theta_1_calcd)): + for j in range(len(theta_2_calcd)): + angle_pairs_list.append((theta_1_calcd[i], theta_2_calcd[j])) + angle_pairs_list = list(set(angle_pairs_list)) + # iterate through the list and check if a particular pair actually produces the requested z-vector orientation joint_angles_list = [] - # iterate through all the possible combinations of (theta_1 , theta_2) - for i in range(len(theta_1_pair)): - for j in range(len(theta_2_pair)): - # rotate an identity matrix using the custom tool kinematic model and the (theta_1, theta_2) - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1_pair[i], theta_2_pair[j], 0, matrix_in,'inv') - # the resulting tool-z vector for this pair of (theta_1, theta_2) is found in the third column - tool_z_would_be = np.array([t_out[0,2], t_out[1,2], t_out[2,2]]) - log.debug('tool_z_would_be: %s', tool_z_would_be) - # calculate the difference of the respective elements - tool_z_diff = tool_z_req - tool_z_would_be - # and check if all elements are within [-epsilon,epsilon] - match = np.all((tool_z_diff > -epsilon) & (tool_z_diff < epsilon)) - log.debug('Is the tool-Z-vector close enough ? %s', match) - if match: - # check if we already have this particular pair in the list - if not (theta_1_pair[i], theta_2_pair[j]) in joint_angles_list: - log.debug('Appending (theta_1_pair, theta_2_pair) %s', (degrees(theta_1_pair[i]), degrees(theta_2_pair[j]))) - joint_angles_list.append((theta_1_pair[i], theta_2_pair[j])) - log.info('Found valid joint angles: %s', joint_angles_list) - if joint_angles_list: - return joint_angles_list - #return joint_angles_list[-1] - else: - return None, None + for i in range(len(angle_pairs_list)): + debug_msg = (f' Checking angle pair {i}: ({angle_pairs_list[i][0]:.4f}, {angle_pairs_list[i][1]:.4f}) ' + f'({degrees(angle_pairs_list[i][0]):.4f}°, {degrees(angle_pairs_list[i][1]):.4f}°)') + log.debug(debug_msg) + # we start with an identity matrix (ie oriented to world) + matrix_in = np.asmatrix(np.identity(4)) + try: + direction = kins_calc_transformation_get_direction() + except Exception as error: + log.error('kins_calc_transformation_get_direction, %s', error) + try: + matrix_out = kins_calc_transformation_matrix(angle_pairs_list[i][0], angle_pairs_list[i][1], 0, matrix_in, direction) + except Exception as error: + log.error('kins_calc_transformation_matrix, %s', error) + # the resulting z-vector for this pair of (theta_1, theta_2) is found in the third column + z_vector_would_be = np.array([matrix_out[0,2], matrix_out[1,2], matrix_out[2,2]]) + # calculate the difference of the respective elements + z_vector_diff = z_vector_req - z_vector_would_be + log.debug(' z_vector_diff: %s', z_vector_diff) + # and check if all elements are within [-epsilon,epsilon] + match_z = np.all((z_vector_diff > -epsilon) & (z_vector_diff < epsilon)) + log.debug(' Is the z-vector-vector close enough ? %s', match_z) + if match_z: + joint_angles_list.append((angle_pairs_list[i][0], angle_pairs_list[i][1])) + for (theta_1, theta_2) in joint_angles_list: + log.debug(f'Returning valid joint angles found: {degrees(theta_1):.4f}°, {degrees(theta_2):.4f}°') + return joint_angles_list # returns radians + def calc_shortest_distance(pos, trgt, mode): - # calculate the shortest distance in [-180°, 180°] - # eg if pos=170° and trgt=-170° then dist will be 20° - # If the operator requests positive or negative rotation - # we may need to return the long distance instead - log.debug('Got (pos, trgt): %s', (pos, trgt)) + pos = degrees(pos) + trgt = degrees(trgt) + # calculate the shortest distance in [-180°, 180°] eg if pos=170° and trgt=-170° then dist will be 20° + # If the operator requests positive or negative rotation we may need to return the long distance instead + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) dist_short = (trgt - pos + 180) % 360 - 180 # calculate short and long distance if dist_short >= 0: # ie dist_long should be negative dist_long = -(360 - dist_short) else: dist_long = 360 + dist_short - log.debug('Calculated (dist_short, dist_long): %s', (dist_short, dist_long)) + log.debug(f' Calculated dist_short: {dist_short:.4f}°, dist_long: {dist_long:.4f}°') if mode == 1: # positive rotation only, ie we want a positive distance if dist_short >= 0: # ie we want this one dist = dist_short else: # ie we need to go the other way dist = dist_long - if mode == 2: # negative rotation only ie we want a positive distance - if dist_short >= 0: # ie we need to go the other way + elif mode == 2: # negative rotation only ie we want a positive distance + if dist_short > 0: # ie we need to go the other way dist = dist_long else: # ie we want this one dist = dist_short else: # mode = 0 ie we want the shortest distance either way dist = dist_short - log.debug('Distance returned: %s', dist) - return dist + log.debug(f'Returning distance: {dist:.4f}°') + return radians(dist) -# this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] -# from a given position in [min_limit, max_limit], returns the optimized target angle and the distance -# from the given position to that target angle -def calc_rotary_move_with_joint_limits(position, target, max_limit, min_limit, mode): - pos = degrees(position) - trgt = degrees(target) - log.debug('(Current_pos, target): %s', (pos, trgt)) +def calc_rotary_move_with_joint_limits(pos, trgt, max_limit, min_limit, mode): # expects radians + # this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] + # from a given position in [min_limit, max_limit], returns the optimized target angle and the distance + # from the given position to that target angle + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + log.debug(f' Current position: {degrees(pos):.4f}°, target position: {degrees(trgt):.4f}°') # calculate the shortest distance from position to target for the strategy given by # the operator (ie shortest (= default), positive rotation only, negative rotation only ) dist = calc_shortest_distance(pos, trgt, mode) # check that the result is within the rotary axis limits defined in the ini file if dist >= 0: # shortest way is in the positive direction if (pos + dist) <= max_limit: # if the limits allow we rotate the joint in the positive sense - log.debug('Max_limit OK, target changed to: %s', (pos + dist)) + log.debug(f' Max_limit OK, setting target to: {degrees(pos + dist):.4f}°') theta = pos + dist - else: # if positive limits would be exceeded we need to go the longer wey in the other direction + else: # if positive limits would be exceeded we need to go the longer way in the other direction + log.debug(f' Maximum axis limit of {degrees(max_limit):.4f} would be violated.') if mode == 0: - log.debug('Max_limit reached, target remains: %s', trgt) + dist = dist - 2*pi + log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') theta = trgt else: # if the rotation direction was set by the operator then we can not change direction + log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') theta = None - dist = None else: # shortest way is in the negative direction if (pos + dist) >= min_limit: # if the limits allow we rotate the joint in the negative sense - log.debug('Min_limit OK, target changed to: %s', (pos + dist)) + log.debug(f' Min_limit OK, setting target to: {degrees(pos + dist):.4f}°') theta = pos + dist else: # if negative limits would be exceeded we need to go the longer way int the other direction + log.debug(f' Minimum axis limit of {degrees(min_limit):.4f} would be violated.') if mode == 0: - log.debug('Min_limit reached, target remains: %s', trgt) + dist = dist + 2*pi + log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') theta = trgt else: # if the rotation direction was set by the operator then we can not change direction + log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') theta = None - dist = None + if theta is not None: + log.debug(f'Returning: angle {degrees(theta):.4f}° with distance {degrees(dist):.4f}° for requested mode {mode:.0f}\n') # we also attach the distance for this particular move and mode - log.debug('Angle and distance returned: %s, %s', theta, dist) - return theta, dist + return theta, dist # returns radians -# this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves -# in (min_limit, max_linit) from the current joint positions using the orient_mode set by -# the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only -def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): +def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): # expects radians + # this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves + # in (min_limit, max_linit) from the current joint positions using the orient_mode set by + # the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global primary_min_limit, primary_max_limit, secondary_min_limit, secondary_max_limit global orient_mode # get the current joint positions - prim_pos, sec_pos = get_current_rotary_positions(self) + prim_pos, sec_pos = get_current_rotary_positions(self) # returns radians # we want to return a list of angles that are optimized for the orient_mode and the # rotary axes limits as set in the ini file target_dist_list= [] for prim_trgt, sec_trgt in possible_prim_sec_angle_pairs: - # primary joint, here we apply the orient mode requested by the operator + # For the priortized joint we apply the orient mode requested by the operator + # the other we optimize for shortest move + if optimization_priority == 2: + primary_strategy = 0 + secondary_strategy = orient_mode + else: + primary_strategy = orient_mode + secondary_strategy = 0 + # primary joint prim_move, prim_dist = calc_rotary_move_with_joint_limits(prim_pos, prim_trgt, primary_max_limit, primary_min_limit, - orient_mode) - # secondary joint, here we want the shortest move (although we could also apply a strategy here) + primary_strategy) + # secondary joint sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, secondary_max_limit, secondary_min_limit, - 0) + secondary_strategy) # if a solution has been found for this particular pair then we add it to the list if not (prim_move == None) and not (sec_move == None): target_dist_list.append(((prim_move, sec_move),(prim_dist, sec_dist))) - log.debug('Assembled target_dist_list: %s',target_dist_list) - return target_dist_list + for ((prim_move, sec_move),(prim_dist, sec_dist)) in target_dist_list: + debug_msg = (f'Returning prim_move: {degrees(prim_move):.4f}°, sec_move: {degrees(sec_move):.4f}°, ' + f'prim_dist: {degrees(prim_dist):.4f}°, sec_dist: {degrees(sec_dist):.4f}°') + log.debug(debug_msg) + return target_dist_list # returns radians -# find the optimal joint move from current to target positions in the list -# for this we look at the primary joint move only -# orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only -# For orient_mode=(1,2): If no move can be found within joint limits we return None def calc_optimal_joint_move(self, possible_prim_sec_angle_pairs): + # find the optimal joint move from current to target positions in the list + # orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only + # For orient_mode=(1,2): If no move can be found within joint limits we return None + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global orient_mode # this returns a list with all moves ((prim_move, sec_move),(prim_dist, sec_dist)) that # will result in correct tool orientation, stay within the rotary axis limits and respect the # orient_mode if set by the operator valid_joint_moves_and_distances = calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs) + if len(valid_joint_moves_and_distances) < 1: + log.error(f' No valid joint moves found.') + return (None, None) # now we need to pick and return the (primary angle, secondary angle) that results in the - # shortest move of the primary joint + # shortest move of the prioritized joint (theta_1, theta_2) = (None, None) - dist = 3600 + joint = optimization_priority - 1 + dist = 10 # some large initial value for trgt_angles, dists in valid_joint_moves_and_distances: - if orient_mode == 0 and fabs(dists[0]) < fabs(dist): # shortest move requested + if orient_mode == 0 and fabs(dists[joint]) < fabs(dist): # shortest move requested (theta_1, theta_2) = trgt_angles dist = dists[0] - elif orient_mode == 1 and fabs(dists[0]) < fabs(dist) and dists[0] >= 0: # positive primary rotation only + elif orient_mode == 1 and fabs(dists[joint]) < fabs(dist) and dists[joint] >= 0: # positive primary rotation only (theta_1, theta_2) = trgt_angles dist = dists[0] - elif orient_mode == 2 and fabs(dists[0]) < fabs(dist) and dists[0] <= 0: # negative primary rotation only + elif orient_mode == 2 and fabs(dists[joint]) < fabs(dist) and dists[joint] <= 0: # negative primary rotation only (theta_1, theta_2) = trgt_angles dist = dists[0] - log.debug('Shortest move selected for (orient_mode, theta_1, theta_2): %s', (orient_mode, theta_1, theta_2)) - return theta_1, theta_2 - - -# calculates the required pre-rotation around tool-z so the tool-x matches the requested -# orientation after rotation of the spindle joints -def kins_calc_pre_rot(self, theta_1, theta_2, tool_x_req, tool_z_requested): - # tolerance setting for check if tool-x-vector needs to be rotated at all + if theta_1 is not None: + debug_msg = (f'Returning shortest move selected for orient_mode {orient_mode:.0f}: ' + f'primary: {degrees(theta_1):.4f}°, secondary: {degrees(theta_2):.4f}°\n') + log.debug(debug_msg) + return theta_1, theta_2 # returns radians + + +def calc_virtual_rotation(theta_1, theta_2, x_vector_req, z_vector_req, matrix_in, direction): # expects radians + # calculates a required virtual-rotation around tool- or work-z so the x-vector matches the requested + # orientation after rotation + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + # tolerance setting for check if x-vector-vector needs to be rotated at all epsilon = 0.00000001 - log.info("Tool-x-requested: %s", tool_x_req) - # we need to calculate the current tool-x vector with the given rotations using - # the transformation matrix from our custom tool kinematic - log.debug("joint angles (secondary, primary) in radians given: %s", (theta_2, theta_1)) - log.debug("joint angles (secondary, primary) in degrees given: %s", (theta_2*180/pi, theta_1*180/pi)) - # run the identity matrix through the tool kinematic transformation in the requested direction - # using the given joint angles and pre-rotation zero - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1, theta_2, 0, matrix_in,'inv') - # the tool-x vector for the given machine joint rotations is found directly in the first column - tool_x_is = [t_out[0,0], t_out[1,0], t_out[2,0]] - log.debug("tool-x after machine rotation would be: %s", tool_x_is) - # we calculate the angular difference between the two vectors so we can 'pre-rotate' - # around tool-z to get the requested tool-x vector after machine rotation + log.info(" x-vector-requested: %s", x_vector_req) + debug_msg = (f' got joint angles: primary {theta_1:.4f} {degrees(theta_1):.4f}°, ' + f'secondary {theta_2:.4f}° {degrees(theta_2):.4f}°') + log.debug(debug_msg) + # run matrix_in through the kinematic transformation in the requested direction + # using the given joint angles and zero virtual-rotation + try: + matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, 0, matrix_in, direction) + except Exception as error: + log.error('calc_virtual_rotation, %s', error) + # the x-vector for the given machine joint rotations is found directly in the first column + x_vector_is = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] + log.debug(" X-vector after machine rotation would be: %s", x_vector_is) + # we calculate the angular difference between the two vectors so we can add a virtual rotation + # around z-vector or work-z to match the requested x orientation after machine rotation # just to be sure we normalize the two vectors - tool_x_is = tool_x_is / np.linalg.norm(tool_x_is) - tool_x_req = tool_x_req / np.linalg.norm(tool_x_req) + x_vector_is = x_vector_is / np.linalg.norm(x_vector_is) + x_vector_req = x_vector_req / np.linalg.norm(x_vector_req) # check if the x-vector is already in the required orientation (ie parallel) - log.debug("check if vectors are parallel: %s", np.dot(tool_x_is,tool_x_req)) - if np.dot(tool_x_is,tool_x_req) > 1 - epsilon: - log.info("Tool x-vector already oriented, setting pre-rotation = 0") - # if we are already parallel then we don't need to pre-rotate - pre_rot = 0 + log.debug(" checking if vectors are parallel: %s", np.dot(x_vector_is,x_vector_req)) + if np.dot(x_vector_is, x_vector_req) > 1 - epsilon: + log.info(" X-vector already oriented, setting virtual-rotation = 0") + # if we are already parallel then we don't need to add a virtual rotation + virtual_rot = 0 else: # we can use the cross product to determine the direction we need to rotate - cross = np.cross(tool_x_req, tool_x_is) - log.debug("cross product (tool_x_req, tool_x_is): %s", cross) - log.info("Tool_z_requested: %s", tool_z_requested) - pre_rot = np.arccos(np.dot(tool_x_req, tool_x_is)) - log.debug('base pre_rot: %s', pre_rot) + cross = np.cross(x_vector_req, x_vector_is) + log.debug(" cross product (x_vector_req, x_vector_is): %s", cross) + virtual_rot = np.arccos(np.dot(x_vector_req, x_vector_is)) + log.debug(f' raw virtual_rot: {virtual_rot:.4f} {degrees(virtual_rot):.4f}°') # To find out which quadrant we need the angle to be in we create a list of them all - pre_rot_list = [pre_rot, -pre_rot, 2*pi-pre_rot, -(2*pi-pre_rot)] - log.debug('pre_rot_list: %s',pre_rot_list) - # then we run all of them through the kinematic model and see which gives us - # the requested tool-x-vector - for pre_rot in pre_rot_list: + virtual_rot_list = [virtual_rot, -virtual_rot, 2*pi-virtual_rot, -(2*pi-virtual_rot)] + log.debug(' Got possible virtual_rot angles: ' + ' '.join("{:.4f}°".format(degrees(angle)) for angle in virtual_rot_list)) + # then we run all of them through the kinematic model and see which gives us the requested x-vector-vector + for virtual_rot in virtual_rot_list: + log.debug(f' Checking virtual_rot = {degrees(virtual_rot):.4f}°') zeta = 0.0001 - # run the identity matrix through the tool kinematic transformation in the requested direction - # using the given joint angles and pre-rotation angle in the list - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in,'inv') - # the tool-x vector for the given primary and secondary rotations is found directly in the first column - tool_x_would_be = [t_out[0,0], t_out[1,0], t_out[2,0]] - log.debug('tool_x_would_be: %s', tool_x_would_be) + # run the identity matrix through the kinematic transformation in the requested direction + # using the given joint angles and virtual-rotation angle in the list + try: + matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) + except Exception as error: + log.error('calc_virtual_rotation, %s', error) + # the oriented x-vector is found directly in the first column + x_vector_would_be = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] + log.debug(' x_vector_would_be: %s', x_vector_would_be) # calculate the difference of the respective elements - tool_x_diff = tool_x_req - tool_x_would_be + x_vector_diff = x_vector_req - x_vector_would_be # and check if all elements are within [-epsilon,epsilon] - match = np.all((tool_x_diff > -zeta) & (tool_x_diff < zeta)) - log.debug('Is the tool-X-vector close enough ? %s', match) + match = np.all((x_vector_diff > -zeta) & (x_vector_diff < zeta)) + log.debug(' Is the X-vector close enough ? %s', match) if match: # if we have a match we leave the loop and use this angle break - log.info("Pre-rotation calculated [deg]: %s", degrees(pre_rot)) - # return pre_rot in radians - return pre_rot - - -# transforms a 4x4 input matrix using the current tool transformation matrix -# (forward or inverse) using the kinematic model of the machine -def kins_calc_tool_transformation(self, matrix_in, theta_1=None, theta_2=None, pre_rot=None, direction='fwd'): - global kins_pre_rotation - # if no angle values have been passed we get the current joint positions - if theta_2 == None or theta_1 == None: - # read current spindle rotary angles and convert to radians - theta_1, theta_2 = get_current_rotary_positions(self) - else: - log.debug("got for secondary joint: %s", theta_2) - log.debug("got for primary joint: %s", theta_1) - # pre-rot is the virtual rotary axis around the tool-z axis to align the tool-x axis - # if no pre-rot angle is passed then we use the currently active value - if pre_rot == None: - pre_rot = hal.get_value(kins_pre_rotation ) - log.debug("current pre-rot: %s", pre_rot) - else: - log.debug("requested pre-rot value [DEG]): %s", degrees(pre_rot)) - # run the input matrix through the tool kinematic transformation in the requested direction - # using the current joint angles and pre-rotation as requested - matrix_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction) - return matrix_out - - -# define the basic rotation matrices, used for euler twp modes -def Rx(th): - return np.array([[1, 0 , 0 ], - [0, cos(th), -sin(th)], - [0, sin(th), cos(th)]]) - -def Ry(th): - return np.array([[ cos(th), 0, sin(th)], - [ 0 , 1, 0 ], - [-sin(th), 0, cos(th)]]) - -def Rz(th): - return np.array([[cos(th), -sin(th), 0], - [sin(th), cos(th), 0], - [0 , 0 , 1]]) + log.info(f'Returning virtual-rotation calculated {degrees(virtual_rot):.4f}°') + return virtual_rot # returns radians -# returns the rotation matrices for given order and angles -def twp_calc_euler_rot_matrix(th1, th2, th3, order): - log.debug("euler order requested: %s", order) - log.debug("angles given (th1, th2 , th3): %s", (th1, th2, th3)) - th1 = radians(th1) - th2 = radians(th2) - th3 = radians(th3) - if order == '131': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) - elif order=='121': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) - elif order=='212': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) - elif order=='232': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) - elif order=='323': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) - elif order=='313': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) - elif order=='123': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) - elif order=='132': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) - elif order=='213': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) - elif order=='231': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) - elif order=='321': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) - elif order=='312': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) - log.debug('euler rotation as matrix: \n %s', matrix) - return matrix +def calc_twp_matrix_from_joint_position(self, matrix_in, virtual_rot, direction): # expects radians + # transforms a 4x4 input matrix using the current transformation matrix + # (forward or inverse) using the kinematic model of the machine + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global kins_virtual_rotation + # read current spindle rotary angles (radians) + theta_1, theta_2 = get_current_rotary_positions(self) + # virtual-rot is the virtual rotary axis around the z-vector or work-z axis to align the x-vector + log.debug(f" requested virtual-rot value {degrees(virtual_rot):.4f}°") + # run matrix_in through the kinematic transformation in the requested direction + # using the current joint angles and virtual-rotation as requested + try: + twp_matrix = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) + except Exception as error: + log.error('calc_twp_matrix_from_joint_position, %s', error) + return twp_matrix -# The tilted-work-plane is created in identity mode and must NOT be updated after a switch -def gui_update_twp(self): +def gui_update_twp(): + # The tilted-work-plane is created in identity mode and must NOT be updated after a switch + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global twp_matrix, saved_work_offset # twp origin as vector (in world coords) from current work-offset to the origin of the twp - hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) - hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) - hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) - # twp x-vector - hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) - hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) - hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) - # twp z-vector - hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) - hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) - hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) + try: + hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) + hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) + hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) + # twp x-vector + hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) + hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) + hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) + # twp z-vector + hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) + hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) + hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) + except Exception as error: + log.error('gui_update_twp failed, %s', error) # publish the twp offset coordinates in world coordinates (ie identity) [work_offset_x, work_offset_y, work_offset_z] = saved_work_offset - log.debug("Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) + log.debug(" Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) # this is used to translate the rotated twp to the correct position # care must be taken that only the work_offsets in identity mode are sent as that is - # what the model uses. The visuals for the offsets are created then rotated according to - # the rotary joint position and then translated. + # what the model uses. The visuals for the offsets are created in the origin, + # then rotated according to the rotary joint position and then translated. # The twp has to be rotated out of the machine xy plane using the g68.2 parameters and is then # translated by the offset values of the identity mode. - hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) - hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) - hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) + try: + hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) + hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) + hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) + except Exception as error: + log.error('gui_update_twp failed, %s', error) # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 # as LinuxCNC seems to revert to G54 as the default system def get_current_work_offset(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) # get which offset is active (g54=1 .. g59.3=9) active_offset = int(self.params[5220]) current_work_offset_number = active_offset @@ -702,11 +530,12 @@ def get_current_work_offset(self): co_x = self.params[work_offset_x] co_y = self.params[work_offset_y] co_z = self.params[work_offset_z] - current_work_offset = [co_x, co_y, co_z] + current_work_offset = (co_x, co_y, co_z) return [current_work_offset_number, current_work_offset] def get_current_rotary_positions(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global joint_letter_primary, joint_letter_secondary if joint_letter_primary == 'A': theta_1 = radians(self.AA_current) @@ -714,7 +543,7 @@ def get_current_rotary_positions(self): theta_1 = radians(self.BB_current) elif joint_letter_primary == 'C': theta_1 = radians(self.CC_current) - log.debug('Current position Primary joint: %s', degrees(theta_1)) + log.debug(f' Current position Primary joint: {degrees(theta_1):.4f}°') # read current spindle rotary angles and convert to radians if joint_letter_secondary == 'A': theta_2 = radians(self.AA_current) @@ -722,45 +551,32 @@ def get_current_rotary_positions(self): theta_2 = radians(self.BB_current) elif joint_letter_secondary == 'C': theta_2 = radians(self.CC_current) - log.debug('Current position Secondary joint: %s', degrees(theta_2)) + log.debug(f' Current position Secondary joint: {degrees(theta_2):.4f}°') return theta_1, theta_2 -# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] -def point_to_matrix(point): - # start with a 4x4 identity matrix and add the point vector to the 4th column - matrix = np.identity(4) - [matrix[0,3], matrix[1,3], matrix[2,3]] = point - matrix = np.asmatrix(matrix) - return matrix - - -# extracts the point vector form a given 4x4 transformation matrix -def matrix_to_point(matrix): - point = (matrix[0,3],matrix[1,3],matrix[2,3]) - return point - - -def reset_twp_params(self): - global pre_rot, twp_matrix, twp_flag, twp_build_params - pre_rot = 0 +def reset_twp_params(): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global virtual_rot, twp_matrix, twp_flag, twp_build_params + virtual_rot = 0 # we must not change tool kins parameters when TOOL kins are active or we get sudden joint position changes - # ie don't do this: kins_comp_set_pre_rot(self,0)! + # ie don't do this: kins_comp_set_virtual_rot(0)! twp_flag = [] twp_build_params = {} - log.info("Resetting TWP-matrix") + log.info(" Resetting TWP-matrix") twp_matrix = np.asmatrix(np.identity(4)) -# Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) -# (some controllers offer an optional P-word to give preferred rotation directions this is not implemented yet) -# Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but -# because we need self.execute() to switch the WCS properly this remap needs to be called from -# an ngc reamp that contains a quebuster before calling this code -# IMPORTANT: -# The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called -# (ie do it in the ngc remap mentioned above!) -def g53x_core(self): - global saved_work_offset, twp_matrix, twp_flag, pre_rot + +def g53n_core(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + # Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) + # Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the + # beginning but because we need self.execute() to switch the WCS properly this remap needs to be called from + # an ngc reamp that contains a quebuster before calling this code. + # IMPORTANT: + # The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called + # (ie do it in the ngc remap mentioned above!) + global saved_work_offset, twp_matrix, twp_flag, virtual_rot global joint_letter_primary, joint_letter_secondary, twp_error_status global orient_mode if self.task == 0: # ignore the preview interpreter @@ -769,112 +585,142 @@ def g53x_core(self): if not hal.get_value(twp_is_defined): # reset the twp parameters - reset_twp_params(self) - msg = "G53.x: No TWP defined." - log.debug(msg) + reset_twp_params() + msg = "G53.n: No TWP defined." + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - elif hal.get_value(twp_is_active): # reset the twp parameters - reset_twp_params(self) - msg = "G53.x: TWP already active" - log.debug(msg) + reset_twp_params() + msg = "G53.n: TWP already active" + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # Check if any words have been passed with the respective G53.x command + # Check if any words have been passed with the respective G53.n command c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 x = c.i_number if c.i_flag else None y = c.j_number if c.j_flag else None z = c.k_number if c.k_flag else None - log.debug('G53.x Words passed: (P, X,Y,Z): %s', (p,x,y,z)) + log.debug(' G53.n Words passed: (P, X,Y,Z): %s', (p,x,y,z)) + if p not in [0,1,2]: - # reset the twp parameters - reset_twp_params(self) - msg = "G53.x : unrecognised P-Word found." - log.debug(msg) + # reset the twp parameters + reset_twp_params() + msg = "G53.n : unrecognised P-Word found." + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR orient_mode = p - # calculate the required rotary joint positions and pre_rotation for the requested tool-orientation + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + # calculate all possible pairs of (primary, secondary) angles to matches the requested orientation try: - tool_z_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - # calculate all possible pairs of (primary, secondary) angles so our tool-z vector matches the requested tool-z # angles are returned in [-pi,pi] - possible_prim_sec_angle_pairs = kins_calc_jnt_angles(self, tool_z_requested) - # An excepton will occur if the requested tool orientation cannot be achieved with the kinematic at hand + possible_prim_sec_angle_pairs = calc_joint_angles(z_vector_requested, x_vector_requested) # returns radians except Exception as error: - log.error('G53.x: Calculation failed, %s', error) - possible_prim_sec_angle_pairs = [] - if not possible_prim_sec_angle_pairs: - # reset the twp parameters - reset_twp_params(self) - msg = "G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x" - log.debug(msg) + log.error('calc_joint_angles, %s', error) + # reset the twp parameters + reset_twp_params() + msg = ("G53.n ERROR: Calculation of joint angles has failed. -> aborting G53.n") + log.debug(' ' + msg) + emccanon.CANON_ERROR(msg) + yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed + yield INTERP_EXIT # w/o this the error does not abort a running gcode program + return INTERP_ERROR + + if possible_prim_sec_angle_pairs == []: + # reset the twp parameters + log.error('G53.n: No possible primary/secondary angle pairs found.') + reset_twp_params() + msg = "G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n" + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # this returns one pair of optimized angles in degrees, or (None, None) if no solution could be found - theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) - if theta_1 == None: + try: + theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) # returns radians + except Exception as error: + log.error('G53.n: Calculation of optimal joint move failed, %s', error) + if theta_1 == None or theta_2 == None: # reset the twp parameters - reset_twp_params(self) - msg = ("G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x") - log.debug(msg) + reset_twp_params() + msg = ("G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n") + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - theta_1 = radians(theta_1) - theta_2 = radians(theta_2) - # calculate the pre-rotation needed so our tool-x vector matches the requested tool-x vector - tool_x_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - pre_rot = kins_calc_pre_rot(self,theta_1, theta_2, tool_x_requested, tool_z_requested) - log.debug("Calculated pre-rotation (pre_rot) to match requested tool-x): %s", pre_rot) + # get the particular conditions to be met for the kinematic at hand + try: + (x_vector_requested, z_vector_requested, matrix_in, direction) = kins_calc_virtual_rot_get_values(x_vector_requested, + z_vector_requested, + twp_matrix) + except Exception as error: + log.error('G53.n: kins_calc_virtual_rot_get_values failed, %s', error) + # calculate the virtual-rotation needed + virtual_rot = calc_virtual_rotation(theta_1, + theta_2, + x_vector_requested, + z_vector_requested, + matrix_in, + direction) # returns radians + log.debug(f" Calculated virtual-rotation to match requested x-vector: {degrees(virtual_rot):.4f}°") + # mark twp-flag as active twp_flag = [0, 'active'] - gui_update_twp(self) - # set the pre-rotation value in the kinematic component - log.debug("G53.x: setting primary, secondary and pre_rotation angles in kinematic component: %s", (degrees(theta_1), degrees(theta_2), degrees(pre_rot))) - hal.set_p(kins_pre_rotation, str(pre_rot)) - hal.set_p(kins_primary_rotation, str(degrees(theta_1))) - hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) - - # calculate the work offset in tool-coords - P = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(saved_work_offset), theta_1, theta_2, pre_rot)) - # get the current twp_origin + gui_update_twp() + + # set the virtual-rotation value in the kinematic component + debug_msg = (f' G53.n: Setting angle values in kins comp to theta1: {degrees(theta_1):.4f}°, ' + f'theta2: {degrees(theta_2):.4f}°, virtual_rot: {degrees(virtual_rot):.4f}°') + log.debug(debug_msg) + try: + kins_set_values(theta_1, theta_2, virtual_rot) + except Exception as error: + log.error('G53.n: kins_set_values failed, %s', error) + + # calculate the work offset in transformed-coordinatess + log.debug(" G53.n: Saved work offset: %s", saved_work_offset) twp_offset = (twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]) - # calculate the twp offset in tool-coords - Q = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(twp_offset), theta_1, theta_2, pre_rot)) - log.debug("G53.x: Setting transformed work-offsets for tool-kins in G59, G59.1, G59.2 and G59.3 to: %s ", P) + try: + new_offset = kins_calc_transformed_work_offset(saved_work_offset, twp_offset, theta_1, theta_2, virtual_rot) + except Exception as error: + log.error('G53.n: Calculation of kins_calc_transformed_work_offset failed, %s', error) + debug_msg = (f' G53.n: Setting transformed work-offsets for twp-kins in G59, G59.1, ' + f'G59.2 and G59.3 to: {new_offset[0]:.4f}, {new_offset[1]:.4f}, {new_offset[2]:.4f}') + log.debug(debug_msg) # set the dedicated TWP work offset values (G53, G53.1, G53.2, G53.3) - self.execute("G10 L2 P6 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P7 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P8 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P9 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - log.debug("G53.x: Moving (secondary and primary) joints to: %s", (degrees(theta_2), degrees(theta_1))) + self.execute("G10 L2 P6 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + self.execute("G10 L2 P7 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + self.execute("G10 L2 P8 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + self.execute("G10 L2 P9 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + + log.debug(f" G53.n: Moving primary joint to {degrees(theta_1):.4f}° and secondary joint to {degrees(theta_2):.4f}° ") if (x,y,z) == (None,None,None): - # Move rotary joints to align the tool with the requested twp - self.execute("G0 %s%f %s%f" % (joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) + # Move rotary joints to align the tool and the requested work plane + self.execute("G0 %s%f %s%f" % (joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) # switch to the dedicated TWP work offsets self.execute("G59", lineno()) - # activate TOOL kinematics + # activate TWP kinematics self.execute("G12.1 P2") if (x,y,z) != (None,None,None): - log.debug('G53.3 called') - self.execute("G0 X%s Y%s Z%s %s%f %s%f" % (x, y, z, joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) + log.debug(' G53.3 called') + self.execute("G0 X%s Y%s Z%s %s%f %s%f" % + (x, y, z, joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) # set twp-state to 'active' (2) self.execute("M68 E2 Q2") yield INTERP_EXECUTE_FINISH @@ -886,24 +732,25 @@ def g53x_core(self): # because we need self.execute() to switch the WCS properly this remap needs to be called from # an ngc that contains a quebuster before calling this code def g69_core(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global twp_flag, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH return INTERP_OK log.info('G69 called') # reset the twp parameters - reset_twp_params(self) - gui_update_twp(self) + reset_twp_params() + gui_update_twp() # set twp-state to 'undefined' (0) self.execute("M68 E2 Q0") yield INTERP_EXECUTE_FINISH return INTERP_OK -# define a virtual tilted-work-plane (twp) that is perpendicular to the current -# tool-orientation +# define a virtual tilted-work-plane (twp) that is perpendicular to the current tool-orientation def g683(self, **words): - global twp_matrix, pre_rot, twp_flag, saved_work_offset_number, saved_work_offset + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global twp_matrix, virtual_rot, twp_flag, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH @@ -917,7 +764,7 @@ def g683(self, **words): if hal.get_value(twp_is_defined): # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg =("G68.3 ERROR: TWP already defined.") log.debug(msg) emccanon.CANON_ERROR(msg) @@ -931,7 +778,7 @@ def g683(self, **words): (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = "G68.3 ERROR: Must be in G54 to define TWP." log.debug(msg) emccanon.CANON_ERROR(msg) @@ -944,23 +791,31 @@ def g683(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested rotation of x-vector around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_flag = [0, 1, 'empty'] # one call to define the twp in this mode - theta_1, theta_2 = get_current_rotary_positions(self) - # calculate tool-prerotation necessary to have tool-x vector in machine xy-plane - pre_rot = kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2 ) - log.info("G68.3: Pre-Rotation calculated for x-vector in machine-xy plane [deg]: %s", pre_rot*180/pi) - # then we need the tool transformation matrix of the current tool orientation with the - # calculated pre-rotation to get the tool-x vector in the machine xy-plane - # for this we take the 4x4 identity matrix and pass it through the inverse tool kinematic - # transformation using the current rotary joint positions and calculated pre-rotation angle - # plus the requested angle of rotation for tool-x from the machine-xy plane + theta_1, theta_2 = get_current_rotary_positions(self) # radians + # calculate virtual rotation to have the oriented x-vector in the direction required for the kinematic at hand + try: + virtual_rot = kins_calc_virtual_rot_for_g683(theta_1, theta_2 ) + except Exception as error: + log.error('remap_func: kins_calc_virtual_rot_for_g683 failed, %s', error) + log.info("G68.3: virtual-Rotation calculated for x-vector in machine-xy plane [deg]: %s", degrees(virtual_rot)) + # then we need to calculate the transformation matrix of the current orientation with the including the + # calculated virtual-rotation. + # for this we take the 4x4 identity matrix and pass it through the kinematic transformation using the + # current rotary joint positions and the calculated virtual-rotation angle plus any additional angle + # passed in the R word of the G68.3 command start_matrix = np.asmatrix(np.identity(4)) - log.info('G68.3: Requested origin rotation [deg]: %s', r) - twp_matrix = kins_calc_tool_transformation(self, start_matrix, None, None, pre_rot + radians(r), 'inv') - log.debug("G68.3: Tool matrix with x-vector in machine xy-plane: \n%s", twp_matrix) + log.info('G68.3: Requested R-word rotation [deg]: %s', degrees(r)) + # the required transformation direction may depend on the kinematic at hand + try: + direction = kins_calc_transformation_get_direction() + except Exception as error: + log.error('kins_calc_transformation_get_direction, %s', error) + twp_matrix = calc_twp_matrix_from_joint_position(self, start_matrix, virtual_rot + r, direction) + log.debug("G68.3: TWP matrix with oriented x-vector: \n%s", twp_matrix) # put the requested origin into the twp_matrix (twp_matrix[0,3], twp_matrix[1,3], twp_matrix[2,3]) = (x, y, z) # update the build state of the twp call @@ -974,13 +829,14 @@ def g683(self, **words): self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + gui_update_twp() return INTERP_OK # definition of a virtual work-plane (twp) using different methods set by the 'p'-word def g682(self, **words): - global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH @@ -994,9 +850,9 @@ def g682(self, **words): if hal.get_value(twp_is_defined): # ie TWP has already been defined # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2: TWP already defined.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1007,9 +863,9 @@ def g682(self, **words): (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = "G68.2 ERROR: Must be in G54 to define TWP." - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1018,7 +874,7 @@ def g682(self, **words): # collect the currently active work offset values (ie g54, g55 or other) saved_work_offset_number = n saved_work_offset = offsets - log.debug("G68.2: Saved work offsets %s", (n, saved_work_offset)) + log.debug(" G68.2: Saved work offsets %s", (n, saved_work_offset)) c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 @@ -1028,9 +884,9 @@ def g682(self, **words): q = str(int(c.q_number if c.q_flag else 313)) if q not in ['121','131','212','232','313','323']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 (P0): No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1040,21 +896,24 @@ def g682(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # parse the requested euler rotation angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1072,34 +931,36 @@ def g682(self, **words): if q not in ['123','132','213','231','312','321']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P1: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # parse the requested origin x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # parse the requested euler rotation angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1111,21 +972,28 @@ def g682(self, **words): twp_flag[2] = 'done' elif p == 2: # twp defined py 3 points on the plane + # TODO implement operator errors as outlined in the twp README + #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively + #- two to the points entered in Q1,Q2,Q3 are identical + #- all three points entered in Q1,Q2,Q3 are on a line + #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and + #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) + # if this is the first call for this mode reset the twp_flag flag if not twp_flag: twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (tool-x) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) + # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) + # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 @@ -1148,9 +1016,9 @@ def g682(self, **words): twp_flag[5] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P2: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1164,36 +1032,38 @@ def g682(self, **words): p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] - log.debug("G68.2 P2: Point 1: %s",p1) - log.debug("G68.2 P2: Point 2: %s",p2) - log.debug("G68.2 P2: Point 3: %s",p3) + log.debug(" G68.2 P2: Point 1: %s",p1) + log.debug(" G68.2 P2: Point 2: %s",p2) + log.debug(" G68.2 P2: Point 3: %s",p3) # build vectors x:P1->P2 and v2:P1->P3 twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug("G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) + log.debug(" G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug("G68.2 P2 (v2): %s",v2) + log.debug(" G68.2 P2 (v2): %s",v2) # normalize the two vectors twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the tool-z vector + # we can use the cross product to calculate the z-vector vector # note: if P3 is on the right side of the vector P1->P2 - # then the tool-z will be below the twp (ie tool-z will be downwards) + # then the z-vector will be below the twp (ie z-vector will be downwards) twp_vect_z = np.cross(twp_vect_x , v2) - log.debug("G68.2 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the tool-y vector + log.debug(" G68.2 P2: Twp_vect_z %s",twp_vect_z) + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.2 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.2 P2: Twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # convert requested origin rotation to radians - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + log.debug(" G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1202,22 +1072,26 @@ def g682(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) + elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) + # TODO implement operator errors as outlined in the twp README + #- G68.2 P3 Q1 and Q2 commands are not entered consecutively + #- one of the vectors is the zero vector + #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: - log.info('first call') + log.info(' first call') twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed twp_build_params = {'q0':[], 'q1':[]} - log.debug('twp_build_params: %s', twp_build_params) + log.debug(' twp_build_params: %s', twp_build_params) if q == 0: # define new origin of the twp x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # first vector (direction of x in the twp) i = c.i_number if c.i_flag else 0 j = c.j_number if c.j_flag else 0 @@ -1232,9 +1106,9 @@ def g682(self, **words): twp_flag[3] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P3: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1249,39 +1123,39 @@ def g682(self, **words): log.debug("(x, y, z): %s", (x, y, z)) log.debug("(i, j, k): %s", (i, j, k)) log.debug("(i1, j1, k1): %s", (i1, j1, k1)) - # build unit vector defining tool-x direction + # build unit vector defining x-vector direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) - log.debug("orth check: %s", orth) + log.debug(" orth check: %s", orth) # the two vectors must be orthogonal - if orth != 0: - reset_twp_params(self) + if orth > 0.001: + reset_twp_params() msg = ("G68.2 P3: Vectors are not orthogonal.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # we can use the cross product to calculate the tool-y vector + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.2 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.2 P3: twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + log.debug(" G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation try: - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - except Exception as e: - log.info('G68.2 P3: twp_origin_rotation failed, %s', e) - log.debug('G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1291,41 +1165,45 @@ def g682(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + + # TODO implement G68.2 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2: No recognised P-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - log.debug("G68.2: twp_flag: %s", twp_flag) - log.debug("G68.2: calls required: %s", twp_flag.count('done')) - log.debug("G68.2: number of calls made: %s", twp_flag.count('done')) + log.debug(" G68.2: twp_flag: %s", twp_flag) + log.debug(" G68.2: calls required: %s", twp_flag.count('done')) + log.debug(" G68.2: number of calls made: %s", twp_flag.count('done')) if twp_flag.count('done') == twp_flag[1]: - log.info('G68.2: requested rotation: %s', radians(r)) - log.info("G68.2: twp-tranformation-matrix: \n%s",twp_matrix) + log.info(' G68.2: requested rotation (degrees): %s', degrees(r)) + log.info(" G68.2: twp-tranformation-matrix: \n%s",twp_matrix) twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info("G68.2: twp origin: %s", twp_origin) + log.info(" G68.2: twp origin: %s", twp_origin) twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info("G68.2: twp vector-x: %s", twp_vect_x) + log.info(" G68.2: twp vector-x: %s", twp_vect_x) twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info("G68.2: twp vector-z: %s", twp_vect_z) + log.info(" G68.2: twp vector-z: %s", twp_vect_z) # set twp-state to 'defined' (1) self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + gui_update_twp() return INTERP_OK + # incremental definition of a virtual work-plane (twp) using different methods set by the 'p'-word def g684(self, **words): - global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH @@ -1339,9 +1217,9 @@ def g684(self, **words): if not hal.get_value(twp_is_active): # ie there is currently no TWP defined # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4: No TWP active to increment from. Run G68.2 or G68.3 first.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1352,18 +1230,16 @@ def g684(self, **words): # Must be in one of the dedicated offset systems for TWP if False: #n < 6: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # store the current TWP to twp_matrix_current = np.matrix.copy(twp_matrix) - c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 @@ -1374,9 +1250,9 @@ def g684(self, **words): if q not in ['121','131','212','232','313','323']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 (P0): No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1386,21 +1262,24 @@ def g684(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 - # parse requested euler angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 + # parse the requested euler rotation angles + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1418,9 +1297,9 @@ def g684(self, **words): if q not in ['123','132','213','231','312','321']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P1: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1430,21 +1309,24 @@ def g684(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # parse the requested euler rotation angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1456,21 +1338,28 @@ def g684(self, **words): twp_flag[2] = 'done' elif p == 2: # twp defined py 3 points on the plane + # TODO implement operator errors as outlined in the twp README + #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively + #- two to the points entered in Q1,Q2,Q3 are identical + #- all three points entered in Q1,Q2,Q3 are on a line + #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and + #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) + # if this is the first call for this mode reset the twp_flag flag if not twp_flag: twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (tool-x) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) + # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) + # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 @@ -1493,9 +1382,9 @@ def g684(self, **words): twp_flag[5] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P2: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1509,38 +1398,38 @@ def g684(self, **words): p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] - log.debug("G68.4 P2: Point 1: %s",p1) - log.debug("G68.4 P2: Point 2: %s",p2) - log.debug("G68.4 P2: Point 3: %s",p3) + log.debug(" G68.4 P2: Point 1: %s",p1) + log.debug(" G68.4 P2: Point 2: %s",p2) + log.debug(" G68.4 P2: Point 3: %s",p3) # build vectors x:P1->P2 and v2:P1->P3 twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug("G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) + log.debug(" G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug("G68.4 P2: (v2) %s", v2) + log.debug(" G68.4 P2: (v2) %s", v2) # normalize the two vectors twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the tool-z vector + # we can use the cross product to calculate the z-vector vector # note: if P3 is on the right side of the vector P1->P2 - # then the tool-z will be below the twp (ie tool-z will be downwards) + # then the z-vector will be below the twp (ie z-vector will be downwards) twp_vect_z = np.cross(twp_vect_x , v2) - log.debug("G68.4 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the tool-y vector + log.debug(" G68.4 P2: Twp_vect_z %s",twp_vect_z) + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.4 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.4 P2: Twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + log.debug(" G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation try: - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - except Exception as e: - log.debug('G68.4 P2: twp_origin_rotation failed ', e) - log.debug('G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1549,9 +1438,13 @@ def g684(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) + elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) + # TODO implement operator errors as outlined in the twp README + #- G68.2 P3 Q1 and Q2 commands are not entered consecutively + #- one of the vectors is the zero vector + #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: @@ -1561,8 +1454,8 @@ def g684(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # first vector (direction of x in the twp) i = c.i_number if c.i_flag else 0 j = c.j_number if c.j_flag else 0 @@ -1577,9 +1470,9 @@ def g684(self, **words): twp_flag[3] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P3: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1594,40 +1487,43 @@ def g684(self, **words): log.debug("(x, y, z) %s", (x, y, z)) log.debug("(i, j, k) %s", (i, j, k)) log.debug("(i1, j1, k1) %s", (i1, j1, k1)) - # build unit vector defining tool-x direction + # build unit vector defining x-vector direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) - log.debug("orth check: %s", orth) + log.debug(" orth check: %s", orth) # the two vectors must be orthogonal if orth != 0: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() ## reset the parameter values #twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed #twp_build_params = {'q0':[], 'q1':[]} msg = ("G68.4 P3: Vectors are not orthogonal.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # we can use the cross product to calculate the tool-y vector + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.4 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.4 P3: twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + log.debug(" G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1637,40 +1533,42 @@ def g684(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + + # TODO implement G68.4 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4: No recognised P-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - log.debug("G68.4: twp_flag: %s", twp_flag) - log.debug("G68.4: calls required: %s", twp_flag.count('done')) - log.debug("G68.4: number of calls made: %s", twp_flag.count('done')) + log.debug(" G68.4: twp_flag: %s", twp_flag) + log.debug(" G68.4: calls required: %s", twp_flag.count('done')) + log.debug(" G68.4: number of calls made: %s", twp_flag.count('done')) if twp_flag.count('done') == twp_flag[1]: - log.info('G68.4: requested rotation %s', radians(r)) - log.info("G68.4: twp_matrix_current: \n%s", twp_matrix_current) - log.info("G68.4: incremental twp_matrix requested: \n%s",twp_matrix) - log.info("G68.4: calculating new twp_matrix...") + log.info(' G68.4: requested rotation (degrees) %s', degrees(r)) + log.info(" G68.4: twp_matrix_current: \n%s", twp_matrix_current) + log.info(" G68.4: incremental twp_matrix requested: \n%s",twp_matrix) + log.info(" G68.4: calculating new twp_matrix...") twp_matrix_new = twp_matrix_current * twp_matrix - log.info("G68.4: twp_matrix_new: \n%s",twp_matrix_new) + log.info(" G68.4: twp_matrix_new: \n%s",twp_matrix_new) twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info("G68.4: twp origin: %s", twp_origin) + log.info(" G68.4: twp origin: %s", twp_origin) twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info("G68.4: twp vector-x: %s", twp_vect_x) + log.info(" G68.4: twp vector-x: %s", twp_vect_x) twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info("G68.4: twp vector-z: %s", twp_vect_z) - log.info("G68.4: incremented twp_matrix: \n%s", twp_matrix_new) + log.info(" G68.4: twp vector-z: %s", twp_vect_z) + log.info(" G68.4: incremented twp_matrix: \n%s", twp_matrix_new) twp_matrix = twp_matrix_new # set twp-state to 'defined' (1) self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + gui_update_twp() return INTERP_OK diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py new file mode 100644 index 00000000000..3689892e44e --- /dev/null +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py @@ -0,0 +1,347 @@ +# This is imported by remap.py and contains twp functionality specific to the +# xyzacb-trsrn config, a machine with primary rotary C and secondary rotary B +# +# +# Copyright ()c) 2025 David Mueller +# +# 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. +# +# +import sys +import numpy as np +from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs +import hal + + +# set up parsing of the inifile +import os +import linuxcnc +# get the path for the ini file used to start this config +inifile = os.environ.get("INI_FILE_NAME") +# instantiate the LinuxCNC ini-parser +config = linuxcnc.ini(inifile) + +## ROTARY JOINT LETTERS +# primary joint +joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() +# secondary joint (ie the one closer to the tool) +joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() +# get the MIN/MAX limits of the respective rotary joint letters +category = 'AXIS_' + joint_letter_primary +primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) +category = 'AXIS_' + joint_letter_secondary +secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) + +## CONNECTIONS TO THE KINEMATIC COMPONENT +# the module is named for the kinematics, its hal pins carry a "_kins" suffix +kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") + '_kins' +kins_nutation_angle = kins_comp + '.nut-angle' +kins_virtual_rotation = kins_comp + '.pre-rot' +kins_primary_rotation = kins_comp + '.primary-angle' +kins_secondary_rotation = kins_comp + '.secondary-angle' + + +# defines the kinematic model for (world <-> tool) coordinates of the machine at hand +# returns 4x4 transformation matrix for given angles and 4x4 input matrix +# NOTE: these matrices must be the same as the ones used to derive the kinematic model +def kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction='fwd'): # expects radians + global kins_nutation_angle + T_in = matrix_in + ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y + Stc = sin(virtual_rot) + Ctc = cos(virtual_rot) + Rtc=np.matrix([[ Ctc, -Stc, 0, 0], + [ Stc, Ctc, 0, 0], + [ 0 , 0 , 1, 0], + [ 0, 0 , 0, 1]]) + + ## Define 4x4 transformation for the primary joint + # get the basic 3x3 rotation matrix (returns array) + Rp = Rz(theta_1) + # add fourth column on the right + Rp = np.hstack((Rp, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rp = np.vstack((Rp, row_4)) + Rp = np.asmatrix(Rp) + + ## Define 4x4 transformation matrix for the secondary joint + # get the basic 3x3 rotation matrix (returns array) + Rs = Ry(theta_2) + # add fourth column on the right + Rs = np.hstack((Rs, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rs = np.vstack((Rs, row_4)) + Rs = np.asmatrix(Rs) + + # Additional definitions for nutating joint + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + Ss = sin(theta_2) + Cs = cos(theta_2) + r = Cs + Sv*Sv*(1-Cs) + s = Cs + Cv*Cv*(1-Cs) + t = Sv*Cv*(1-Cs) + # define rotation matrix for the secondary joint + Rs=np.matrix([[ Cs, -Cv*Ss, Sv*Ss, 0], + [ Cv*Ss, r, t, 0], + [ -Sv*Ss, t, s, 0], + [ 0, 0, 0, 1]]) + + # calculate the transformation matrix for the forward tool kinematic + matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in + # calculate the transformation matrix for the inverse tool kinematic + matrix_tool_inv = Rp*Rs*Rtc*T_in + if direction == 'fwd': + #log.debug("matrix tool fwd: \n", matrix_tool_fwd) + #log.debug("inv would have been: \n", matrix_tool_inv) + return matrix_tool_fwd + elif direction == 'inv': + #log.debug("matrix tool inv: \n", matrix_tool_inv) + #log.debug("fwd would have been: \n", matrix_tool_fwd) + return matrix_tool_inv + else: + return 0 + + +# calculates the primary joint position for a given tool-vector +# Note: this uses functions derived from the custom kinematic +def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): + global primary_min_limit, primary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_1_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + return [0] + else: + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + for i in range(len(theta_2_list)): + theta_2 = theta_2_list[i] + Ss = sin(theta_2) + Cs = cos(theta_2) + t = Sv*Cv*(1-Cs) + p = Sv * Ss + theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) + # since we are using asin() we really have two solutions theta_1 and pi-theta_2 + for theta in [theta_1, transform_to_pipi(pi - theta_1)]: + log.debug(f' Checking possible primary angle {degrees(theta):.4f}° for limit violations.') + if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + theta_1_list.append(theta) + return theta_1_list # returns radians + + +# calculates the secondary joint position for a given tool-vector +# secondary being the joint closest to the tool +# Note: this uses functions derived from the custom kinematic +def kins_calc_secondary(log, z_vector_req, x_vector_req): + global secondary_min_limit, secondary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_2_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + theta_2 = 0 + # This kinematics nutation angle restricts the negative range of Kzz + elif Kzz < 2*Cv*Cv - 1: + log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') + return None + else: + theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + for theta in [theta_2, -theta_2]: + log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') + if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + theta_2_list.append(theta) + return theta_2_list # returns radians + + +# define the order in which the joint angles need to be calculated +def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): + try: + theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + if theta_2_calcd == None: + return (None, None) + try: + theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (theta_1_calcd, theta_2_calcd) # returns radians + + +# calculate the transformed work offset used after 53.n +def kins_calc_transformed_work_offset(current_offset, twp_offset, theta_1, theta_2, virtual_rot): + P = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(current_offset))) + # calculate the twp offset in transformed-coordinates + Q = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(twp_offset))) + transformed_offset = (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]) + return transformed_offset + +# pass required values to the kinematics component +# the module takes the virtual rotation in radians and the two joint angles in +# degrees, the same units the joints themselves are in +def kins_set_values(theta_1, theta_2, virtual_rot): # expects radians + hal.set_p(kins_virtual_rotation, str(virtual_rot)) + hal.set_p(kins_primary_rotation, str(degrees(theta_1))) + hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) + + +# returns angle required to orient the x-vector parallel to the machine-xy plane +# for given machine joint position angles. +# For G68.3 this is the default tool-x direction +# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic +# TODO I don't actually know if this is the correct x orientation for G68.3' +def kins_calc_virtual_rot_for_g683(theta_1, theta_2): + # The idea is that the oriented x-vector is parallel to the machine xy-plane when the + # z component of the x-direction vector is equal to zero + # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation + # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. + # this makes the x-vector of the oriented coords horizontal and the user can set the + # rotation from there using g68.3 r + global kins_nutation_angle + v = radians(hal.get_value(kins_nutation_angle)) + Cv = cos(v) + Sv = sin(v) + Cs = cos(theta_2) + Ss = sin(theta_2) + Cp = cos(theta_1) + Sp = sin(theta_1) + t = Sv*Cv*(1-Cs) + tc = atan2((Sv*Ss),t) + # note: rotation is done using a halpin that feeds into the kinematic component and the + # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) + return tc # returns radians + + +# return the start values required to calculate the virtual rotation +def kins_calc_virtual_rot_get_values(x_vector_requested, z_vector_requested, twp_matrix): + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + matrix_in = np.asmatrix(np.identity(4)) + direction = 'inv' + return (x_vector_requested, z_vector_requested, matrix_in, direction) + + +# If the operator has requested a rotation by passing an R word in the 68.n command we need to +# create a rotation matrix that represents a rotation around the Z-axis of the TWP plane +def kins_calc_twp_origin_rot_matrix(r): # expects radians + # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + twp_origin_rot_matrix = calc_euler_rot_matrix(0, r, 0, '131') + + return twp_origin_rot_matrix + + +# This returns which transformation to use when checking calculated angles +# and when calculating the twp_matrix for G68.3 +def kins_calc_transformation_get_direction(): + return 'inv' + + +# returns the pin name for the virtual rotation in the kinematics component +def kins_get_current_virtual_rot(): + current_virtual_rot = hal.get_value(kins_virtual_rotation) + return current_virtual_rot # returns radians + + + + + + + +# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] +def point_to_matrix(point): + # start with a 4x4 identity matrix and add the point vector to the 4th column + matrix = np.identity(4) + [matrix[0,3], matrix[1,3], matrix[2,3]] = point + matrix = np.asmatrix(matrix) + return matrix + +# extracts the point vector form a given 4x4 transformation matrix +def matrix_to_point(matrix): + point = (matrix[0,3],matrix[1,3],matrix[2,3]) + return point + + +# this is from 'mika-s.github.io' +# transforms a given angle to the interval of [-pi,pi] +def transform_to_pipi(input_angle): + def truncated_remainder(dividend, divisor): + divided_number = dividend / divisor + divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) + remainder = dividend - divisor * divided_number + return remainder + + revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) + p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) + p2 = (np.sign(np.sign(input_angle) + + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi + output_angle = p1 - p2 + return output_angle + + +# define the basic rotation matrices, used for euler twp modes +def Rx(th): + return np.array([[1, 0 , 0 ], + [0, cos(th), -sin(th)], + [0, sin(th), cos(th)]]) + +def Ry(th): + return np.array([[ cos(th), 0, sin(th)], + [ 0 , 1, 0 ], + [-sin(th), 0, cos(th)]]) + +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) + + +# returns the rotation matrices for given order and angles +def calc_euler_rot_matrix(th1, th2, th3, order): + if order == '131': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) + elif order=='121': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) + elif order=='212': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) + elif order=='232': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) + elif order=='323': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) + elif order=='313': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) + elif order=='123': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) + elif order=='132': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) + elif order=='213': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) + elif order=='231': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) + elif order=='321': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) + elif order=='312': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) + return matrix diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 06b9cd5d23f..2be585e6ef8 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini @@ -39,7 +39,7 @@ SUBROUTINE_PATH = ../remap_subs:../demos REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53x_core + REMAP = M530 modalgroup=10 python=g53n_core REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py new file mode 100644 index 00000000000..f23e29112f5 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py @@ -0,0 +1,350 @@ +# This is imported by remap.py and contains twp functionality specific to the +# xyzbca-trsrn config, a machine with primary rotary C and secondary rotary A +# +# +# Copyright ()c) 2025 David Mueller +# +# 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. +# +# +import sys +import numpy as np +from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs +import hal + + +# set up parsing of the inifile +import os +import linuxcnc +# get the path for the ini file used to start this config +inifile = os.environ.get("INI_FILE_NAME") +# instantiate the LinuxCNC ini-parser +config = linuxcnc.ini(inifile) + +## ROTARY JOINT LETTERS +# primary joint +joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() +# secondary joint (ie the one closer to the tool) +joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() +# get the MIN/MAX limits of the respective rotary joint letters +category = 'AXIS_' + joint_letter_primary +primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) +category = 'AXIS_' + joint_letter_secondary +secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) + +## CONNECTIONS TO THE KINEMATIC COMPONENT +# the module is named for the kinematics, its hal pins carry a "_kins" suffix +kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") + '_kins' +kins_nutation_angle = kins_comp + '.nut-angle' +kins_virtual_rotation = kins_comp + '.pre-rot' +kins_primary_rotation = kins_comp + '.primary-angle' +kins_secondary_rotation = kins_comp + '.secondary-angle' + + +# defines the kinematic model for (world <-> tool) coordinates of the machine at hand +# returns 4x4 transformation matrix for given angles and 4x4 input matrix +# NOTE: these matrices must be the same as the ones used to derive the kinematic model +def kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction='fwd'): # expects radians + global kins_nutation_angle + T_in = matrix_in + ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y + Stc = sin(virtual_rot) + Ctc = cos(virtual_rot) + Rtc=np.matrix([[ Ctc, -Stc, 0, 0], + [ Stc, Ctc, 0, 0], + [ 0 , 0 , 1, 0], + [ 0, 0 , 0, 1]]) + + ## Define 4x4 transformation for the primary joint + # get the basic 3x3 rotation matrix (returns array) + Rp = Rz(theta_1) + # add fourth column on the right + Rp = np.hstack((Rp, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rp = np.vstack((Rp, row_4)) + Rp = np.asmatrix(Rp) + + ## Define 4x4 transformation matrix for the secondary joint + # get the basic 3x3 rotation matrix (returns array) + Rs = Rx(theta_2) + # add fourth column on the right + Rs = np.hstack((Rs, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rs = np.vstack((Rs, row_4)) + Rs = np.asmatrix(Rs) + + # Additional definitions for nutating joint + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + Ss = sin(theta_2) + Cs = cos(theta_2) + r = Cs + Sv*Sv*(1-Cs) + s = Cs + Cv*Cv*(1-Cs) + t = Sv*Cv*(1-Cs) + # define rotation matrix for the secondary joint + Rs=np.matrix([[ r, -Cv*Ss, t, 0], + [ Cv*Ss, Cs, -Sv*Ss, 0], + [ t, Sv*Ss, s, 0], + [ 0, 0, 0, 1]]) + + # calculate the transformation matrix for the forward tool kinematic + matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in + # calculate the transformation matrix for the inverse tool kinematic + matrix_tool_inv = Rp*Rs*Rtc*T_in + if direction == 'fwd': + #log.debug("matrix tool fwd: \n", matrix_tool_fwd) + #log.debug("inv would have been: \n", matrix_tool_inv) + return matrix_tool_fwd + elif direction == 'inv': + #log.debug("matrix tool inv: \n", matrix_tool_inv) + #log.debug("fwd would have been: \n", matrix_tool_fwd) + return matrix_tool_inv + else: + return 0 + + +# calculates the primary joint position for a given tool-vector +# Note: this uses functions derived from the custom kinematic +def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): + global primary_min_limit, primary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_1_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + return [0] + else: + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + for i in range(len(theta_2_list)): + theta_2 = theta_2_list[i] + Ss = sin(theta_2) + Cs = cos(theta_2) + t = Sv*Cv*(1-Cs) + p = Sv * Ss + q = (t*Kzy - p*Kzx)/(t*t + p*p) + theta_1 = asin(q) + # since we are using asin() we really have two solutions theta_1 and pi-theta_2 + for theta in [theta_1, transform_to_pipi(pi - theta_1)]: + if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + theta_1_list.append(theta) + + return theta_1_list + + +# calculates the secondary joint position for a given tool-vector +# secondary being the joint closest to the tool +# Note: this uses functions derived from the custom kinematic +def kins_calc_secondary(log, z_vector_req, x_vector_req): + global secondary_min_limit, secondary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_2_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + theta_2 = 0 + # This kinematics nutation angle restricts the negative range of Kzz + elif Kzz < 2*Cv*Cv - 1: + log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') + return None + else: + theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + # since we are using acos() we really have two solutions theta_1 and -theta_1 + for theta in [theta_2, -theta_2]: + log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') + if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + theta_2_list.append(theta) + + return theta_2_list # returns radians + + +# define the order in which the joint angles need to be calculated +def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): + try: + theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + if theta_2_calcd == None: + return (None, None) + try: + theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (theta_1_calcd, theta_2_calcd) # returns radians + + +# calculate the transformed work offset used after 53.n +def kins_calc_transformed_work_offset(current_offset, twp_offset, theta_1, theta_2, virtual_rot): + P = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(current_offset))) + # calculate the twp offset in transformed-coordinates + Q = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(twp_offset))) + transformed_offset = (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]) + return transformed_offset + +# pass required values to the kinematics component +# the module takes the virtual rotation in radians and the two joint angles in +# degrees, the same units the joints themselves are in +def kins_set_values(theta_1, theta_2, virtual_rot): # expects radians + hal.set_p(kins_virtual_rotation, str(virtual_rot)) + hal.set_p(kins_primary_rotation, str(degrees(theta_1))) + hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) + + +# returns angle required to orient the x-vector parallel to the machine-xy plane +# for given machine joint position angles. +# For G68.3 this is the default tool-x direction +# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic +# TODO I don't actually know if this is the correct x orientation for G68.3' +def kins_calc_virtual_rot_for_g683(theta_1, theta_2): + # The idea is that the oriented x-vector is parallel to the machine xy-plane when the + # z component of the x-direction vector is equal to zero + # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation + # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. + # this makes the x-vector of the oriented coords horizontal and the user can set the + # rotation from there using g68.3 r + global kins_nutation_angle + v = radians(hal.get_value(kins_nutation_angle)) + Cv = cos(v) + Sv = sin(v) + Cs = cos(theta_2) + Ss = sin(theta_2) + Cp = cos(theta_1) + Sp = sin(theta_1) + t = Sv*Cv*(1-Cs) + tc = atan2(-t,(Sv*Ss)) + # note: rotation is done using a halpin that feeds into the kinematic component and the + # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) + return tc # returns radians + + +# return the start values required to calculate the virtual rotation +def kins_calc_virtual_rot_get_values(x_vector_requested, z_vector_requested, twp_matrix): + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + matrix_in = np.asmatrix(np.identity(4)) + direction = 'inv' + return (x_vector_requested, z_vector_requested, matrix_in, direction) + + +# If the operator has requested a rotation by passing an R word in the 68.n command we need to +# create a rotation matrix that represents a rotation around the Z-axis of the TWP plane +def kins_calc_twp_origin_rot_matrix(r): # expects radians + # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + twp_origin_rot_matrix = calc_euler_rot_matrix(0, r, 0, '131') + + return twp_origin_rot_matrix + + +# This returns which transformation to use when checking calculated angles +# and when calculating the twp_matrix for G68.3 +def kins_calc_transformation_get_direction(): + return 'inv' + + +# returns the pin name for the virtual rotation in the kinematics component +def kins_get_current_virtual_rot(): + current_virtual_rot = hal.get_value(kins_virtual_rotation) + return current_virtual_rot # returns radians + + + + + + + +# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] +def point_to_matrix(point): + # start with a 4x4 identity matrix and add the point vector to the 4th column + matrix = np.identity(4) + [matrix[0,3], matrix[1,3], matrix[2,3]] = point + matrix = np.asmatrix(matrix) + return matrix + +# extracts the point vector form a given 4x4 transformation matrix +def matrix_to_point(matrix): + point = (matrix[0,3],matrix[1,3],matrix[2,3]) + return point + + +# this is from 'mika-s.github.io' +# transforms a given angle to the interval of [-pi,pi] +def transform_to_pipi(input_angle): + def truncated_remainder(dividend, divisor): + divided_number = dividend / divisor + divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) + remainder = dividend - divisor * divided_number + return remainder + + revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) + p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) + p2 = (np.sign(np.sign(input_angle) + + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi + output_angle = p1 - p2 + return output_angle + + +# define the basic rotation matrices, used for euler twp modes +def Rx(th): + return np.array([[1, 0 , 0 ], + [0, cos(th), -sin(th)], + [0, sin(th), cos(th)]]) + +def Ry(th): + return np.array([[ cos(th), 0, sin(th)], + [ 0 , 1, 0 ], + [-sin(th), 0, cos(th)]]) + +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) + + +# returns the rotation matrices for given order and angles +def calc_euler_rot_matrix(th1, th2, th3, order): + if order == '131': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) + elif order=='121': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) + elif order=='212': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) + elif order=='232': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) + elif order=='323': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) + elif order=='313': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) + elif order=='123': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) + elif order=='132': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) + elif order=='213': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) + elif order=='231': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) + elif order=='321': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) + elif order=='312': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) + return matrix diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini index d9ae382fefc..d3032855aee 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini @@ -39,7 +39,7 @@ SUBROUTINE_PATH = ../remap_subs:../demos REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53x_core + REMAP = M530 modalgroup=10 python=g53n_core REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 From c8b68920a4caa26d89e68fc8a9868d72330fc724 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 24 Aug 2026 00:26:03 +1000 Subject: [PATCH 19/20] twp: check the primary angle against the primary joint limits kins_calc_primary filtered its candidates against the secondary joint's limits. The function declares primary_min_limit and primary_max_limit as globals and then does not use them, so the intent is not in doubt. It is invisible on both configs in tree, where the primary C is the wider of the two: the runs are byte-identical before and after over eight orientations under G53.1 P0/P1/P2, G53.3, G53.6 and G68.3 on each machine, 36 commands and no errors either way. It bites the other way round, on a machine whose primary is tighter than its secondary, where reachable orientations are rejected for exceeding a limit belonging to the other joint. --- .../xyzacb-trsrn_twp/remap_funcs_twp.py | 2 +- .../xyzbca-trsrn_twp/remap_funcs_twp.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py index 3689892e44e..7c2b498b7b6 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py @@ -142,7 +142,7 @@ def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): # since we are using asin() we really have two solutions theta_1 and pi-theta_2 for theta in [theta_1, transform_to_pipi(pi - theta_1)]: log.debug(f' Checking possible primary angle {degrees(theta):.4f}° for limit violations.') - if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + if degrees(theta) > primary_min_limit and degrees(theta) < primary_max_limit: theta_1_list.append(theta) return theta_1_list # returns radians diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py index f23e29112f5..b1629e7d012 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py @@ -142,7 +142,7 @@ def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): theta_1 = asin(q) # since we are using asin() we really have two solutions theta_1 and pi-theta_2 for theta in [theta_1, transform_to_pipi(pi - theta_1)]: - if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + if degrees(theta) > primary_min_limit and degrees(theta) < primary_max_limit: theta_1_list.append(theta) return theta_1_list From d5bc500918e7380e3e75b3ebb02408eb92def026 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 25 Aug 2026 01:03:48 +1000 Subject: [PATCH 20/20] twp: keep the arc functions inside their domain Asking for a tool vector that lies in a principal plane raises "math domain error" from asin() in kins_calc_primary. The vector reaching that point is a column of a product of rotation matrices, so it is a unit vector only to within rounding, and where one of its components is zero the argument lands on plus or minus one with a rounding error on top and falls outside the domain. Round tripping every reachable orientation, primary and secondary swept in five degree steps, gives 47 failures out of 5184 at the configured nutation of 45 degrees on both machines, and it does not need an unusual nutation angle to appear: 15, 30, 45, 60, 75 and 90 degrees all fail, between 20 and 107 times. Every failure is a tool vector with a component at zero, which is what a G68.2 with I0 or J0 asks for. Clamp the argument where it is within a rounding error of the limit, and leave anything further out to raise, because that is an orientation the machine cannot reach rather than an arithmetic artefact. With the clamp all 5184 orientations solve at every nutation angle tried, on both machines. kins_calc_possible_joint_angles logged such a failure and then fell through to return a variable it had never assigned, so the domain error arrived as an UnboundLocalError over the top of it. Return no solution instead, which is the answer the caller already handles. --- .../xyzacb-trsrn_twp/remap_funcs_twp.py | 32 +++++++++++++++++-- .../xyzbca-trsrn_twp/remap_funcs_twp.py | 32 +++++++++++++++++-- 2 files changed, 60 insertions(+), 4 deletions(-) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py index 7c2b498b7b6..7c2ebd39961 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py @@ -21,6 +21,29 @@ import hal +# asin() and acos() take a value that the trigonometry guarantees is within +# [-1, 1] and that floating point does not. The tool vector reaching here is +# a column of a product of rotation matrices, so it is a unit vector only to +# within rounding, and one ulp of slack in it is enough to put the argument +# outside the domain. A nutation angle of 90 degrees makes that certain +# rather than unlucky: Cv is zero, so t vanishes, and the ratio below reduces +# to Kzy/Ss with nothing left to absorb the slop. +# +# Anything within a rounding error of the limit is pulled back to it. Beyond +# that the request really is out of range and is left to raise, because that +# is a machine that cannot reach the orientation and not an arithmetic +# artefact. +UNIT_EPSILON = 1e-9 + +def clamp_unit(value): + if -1.0 - UNIT_EPSILON <= value <= -1.0: + return -1.0 + if 1.0 <= value <= 1.0 + UNIT_EPSILON: + return 1.0 + return value + + + # set up parsing of the inifile import os import linuxcnc @@ -138,7 +161,7 @@ def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): Cs = cos(theta_2) t = Sv*Cv*(1-Cs) p = Sv * Ss - theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) + theta_1 = asin(clamp_unit((p*Kzy - t*Kzx)/(t*t + p*p))) # since we are using asin() we really have two solutions theta_1 and pi-theta_2 for theta in [theta_1, transform_to_pipi(pi - theta_1)]: log.debug(f' Checking possible primary angle {degrees(theta):.4f}° for limit violations.') @@ -168,7 +191,7 @@ def kins_calc_secondary(log, z_vector_req, x_vector_req): log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') return None else: - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + theta_2 = acos(clamp_unit((Kzz - Cv*Cv)/(1 - Cv*Cv))) for theta in [theta_2, -theta_2]: log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: @@ -182,12 +205,17 @@ def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) except Exception as error: log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + # an orientation this machine cannot reach is 'no solution', which the + # caller already handles. Falling through would raise a second and + # less informative error over the top of this one. + return (None, None) if theta_2_calcd == None: return (None, None) try: theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) except Exception as error: log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (None, None) return (theta_1_calcd, theta_2_calcd) # returns radians diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py index b1629e7d012..abb24726b72 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py @@ -21,6 +21,29 @@ import hal +# asin() and acos() take a value that the trigonometry guarantees is within +# [-1, 1] and that floating point does not. The tool vector reaching here is +# a column of a product of rotation matrices, so it is a unit vector only to +# within rounding, and one ulp of slack in it is enough to put the argument +# outside the domain. A nutation angle of 90 degrees makes that certain +# rather than unlucky: Cv is zero, so t vanishes, and the ratio below reduces +# to Kzy/Ss with nothing left to absorb the slop. +# +# Anything within a rounding error of the limit is pulled back to it. Beyond +# that the request really is out of range and is left to raise, because that +# is a machine that cannot reach the orientation and not an arithmetic +# artefact. +UNIT_EPSILON = 1e-9 + +def clamp_unit(value): + if -1.0 - UNIT_EPSILON <= value <= -1.0: + return -1.0 + if 1.0 <= value <= 1.0 + UNIT_EPSILON: + return 1.0 + return value + + + # set up parsing of the inifile import os import linuxcnc @@ -138,7 +161,7 @@ def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): Cs = cos(theta_2) t = Sv*Cv*(1-Cs) p = Sv * Ss - q = (t*Kzy - p*Kzx)/(t*t + p*p) + q = clamp_unit((t*Kzy - p*Kzx)/(t*t + p*p)) theta_1 = asin(q) # since we are using asin() we really have two solutions theta_1 and pi-theta_2 for theta in [theta_1, transform_to_pipi(pi - theta_1)]: @@ -169,7 +192,7 @@ def kins_calc_secondary(log, z_vector_req, x_vector_req): log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') return None else: - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + theta_2 = acos(clamp_unit((Kzz - Cv*Cv)/(1 - Cv*Cv))) # since we are using acos() we really have two solutions theta_1 and -theta_1 for theta in [theta_2, -theta_2]: log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') @@ -185,12 +208,17 @@ def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) except Exception as error: log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + # an orientation this machine cannot reach is 'no solution', which the + # caller already handles. Falling through would raise a second and + # less informative error over the top of this one. + return (None, None) if theta_2_calcd == None: return (None, None) try: theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) except Exception as error: log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (None, None) return (theta_1_calcd, theta_2_calcd) # returns radians