From 44a1d1d00cb2fa4d07b046a160320aa424ee633c 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/58] 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 8156c90515f..ae9fbe7110b 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4353,7 +4353,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; @@ -6492,6 +6505,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 b241b6167b3..2411aafc0cc 100644 --- a/src/emc/rs274ngc/interp_execute.cc +++ b/src/emc/rs274ngc/interp_execute.cc @@ -324,6 +324,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 6cfefb01228..8f48850dd28 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -210,6 +210,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, @@ -746,6 +748,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 b39157093e8..74b6c43ed42 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 c14064dead9..37d6e2aae1f 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1194,6 +1194,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 */ @@ -1475,6 +1476,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; } @@ -2675,6 +2683,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 254a2d1f867696abfc2b40ede0d5081ed06b73c4 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/58] 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 37d6e2aae1f..304500b3304 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -2071,6 +2071,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 fddf5dfe103e4b27f46c5c688423181f5fc10f47 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/58] 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 9e1453c31c24ec3d3d3a48ea23186e9d942fcf05 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/58] 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 a42099189bf864f81c52ddbb606ec32136796d4a 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/58] 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 ae9fbe7110b..5cb27f29871 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6532,7 +6532,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 b0b9950890abc7d85ce611156909ef1fcda925d6 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/58] 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 c96f9b5aaf284c49ff8068f33b4d1542e66f084c 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/58] 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 d716cdc9a1946d6cba85462984140e7ada6b4e91 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/58] 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 5427c4802f87ef942f8905f01b074854e7670685 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/58] 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 824d0ddb8e58b2bc90cb6fca636a4e59217f9e06 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 23 Aug 2026 14:32:38 +1000 Subject: [PATCH 10/58] docs: add a kinematics conventions chapter A kinematics module reports the controlled point and nothing else, so everything needing the rest of the machine geometry rebuilds it. For xyzacb-trsrn the same chain is written three times: closed-form in xyzacb_trsrn.comp, as homogeneous matrices in the config's remap_funcs_twp.py, and as signed HalRotate calls in the vismach model. The Python copy is kept in step by a comment saying its matrices "must be the same as the ones used to derive the kinematic model". Write down the vocabulary they would need to share: the four frames and which one kinematicsForward() reports in, the rotation sense already stated under Rotational Axes and its ISO 841 equivalent, what conventional-directions costs at its default, and the definition of the tool frame. Tool x is the part worth stating as a rule rather than a formula. The virtual rotation about tool z supplies what a five-axis machine cannot, and the convention is that it leaves tool x parallel to the machine xy-plane; the formula follows from the machine's own secondary rotation matrix, which is why the two nutating configs in tree have different ones. Also anchor the Rotational Axes section so it can be referenced. --- docs/po4a.cfg | 1 + docs/src/Master_Documentation.adoc | 2 + docs/src/Submakefile | 1 + docs/src/gcode/machining-center.adoc | 1 + docs/src/index.tmpl | 1 + docs/src/motion/kinematics-conventions.adoc | 333 ++++++++++++++++++++ 6 files changed, 339 insertions(+) create mode 100644 docs/src/motion/kinematics-conventions.adoc diff --git a/docs/po4a.cfg b/docs/po4a.cfg index 85f3cf55eef..7626ea89001 100644 --- a/docs/po4a.cfg +++ b/docs/po4a.cfg @@ -379,6 +379,7 @@ [type: AsciiDoc_def] src/motion/dh-parameters.adoc $lang:build/adoc/$lang/motion/dh-parameters.adoc [type: AsciiDoc_def] src/motion/dual-pid-example.adoc $lang:build/adoc/$lang/motion/dual-pid-example.adoc [type: AsciiDoc_def] src/motion/external-offsets.adoc $lang:build/adoc/$lang/motion/external-offsets.adoc +[type: AsciiDoc_def] src/motion/kinematics-conventions.adoc $lang:build/adoc/$lang/motion/kinematics-conventions.adoc [type: AsciiDoc_def] src/motion/kinematics.adoc $lang:build/adoc/$lang/motion/kinematics.adoc [type: AsciiDoc_def] src/motion/pid-theory.adoc $lang:build/adoc/$lang/motion/pid-theory.adoc [type: AsciiDoc_def] src/motion/switchkins.adoc $lang:build/adoc/$lang/motion/switchkins.adoc diff --git a/docs/src/Master_Documentation.adoc b/docs/src/Master_Documentation.adoc index 993f6a318bc..12903195a5f 100644 --- a/docs/src/Master_Documentation.adoc +++ b/docs/src/Master_Documentation.adoc @@ -189,6 +189,8 @@ include::ladder/ladder-examples.adoc[] :leveloffset: 2 include::motion/kinematics.adoc[] +include::motion/kinematics-conventions.adoc[] + include::motion/dh-parameters.adoc[] include::motion/5-axis-kinematics.adoc[] diff --git a/docs/src/Submakefile b/docs/src/Submakefile index b585ee4574b..b544081971d 100644 --- a/docs/src/Submakefile +++ b/docs/src/Submakefile @@ -251,6 +251,7 @@ DOC_SRCS_EN := \ ladder/ladder-intro.adoc \ lathe/lathe-user.adoc \ motion/kinematics.adoc \ + motion/kinematics-conventions.adoc \ motion/dh-parameters.adoc \ motion/pid-theory.adoc \ motion/dual-pid-example.adoc \ diff --git a/docs/src/gcode/machining-center.adoc b/docs/src/gcode/machining-center.adoc index e6760936099..27df8428626 100644 --- a/docs/src/gcode/machining-center.adoc +++ b/docs/src/gcode/machining-center.adoc @@ -108,6 +108,7 @@ The U, V and W axes also form a standard right-handed coordinate system. X and U are parallel, Y and V are parallel, and Z and W are parallel (when A, B, and C are rotated to zero). +[[sec:rotational-axes]] === Rotational Axes The rotational axes are measured in degrees as wrapped linear axes in diff --git a/docs/src/index.tmpl b/docs/src/index.tmpl index d82c576d947..99cd603b891 100644 --- a/docs/src/index.tmpl +++ b/docs/src/index.tmpl @@ -158,6 +158,7 @@

  • Kinematics
    • +
  • Kinematics Conventions
  • DH Parameters
  • 5-Axis-Kinematics
  • Switchable Kinematics
    • diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc new file mode 100644 index 00000000000..0520eaeae28 --- /dev/null +++ b/docs/src/motion/kinematics-conventions.adoc @@ -0,0 +1,333 @@ +:lang: en +:toc: + +[[cha:kinematics-conventions]] += Kinematics Conventions + +== Introduction + +A kinematics module answers two questions: where the controlled point is for a +given set of joint positions, and which joint positions put it at a requested +place. `kinematicsForward()` and `kinematicsInverse()` are those two answers. + +Everything else about a machine's geometry is currently rebuilt outside the +module. A tilted work plane needs the direction the tool points in, a +simulation model needs the whole chain of frames, a limit check needs the rate +joints move per unit of commanded motion. For the `xyzacb-trsrn` machine that +geometry is written three times: as closed-form expressions in +`src/hal/components/xyzacb_trsrn.comp`, as homogeneous matrices in the config's +`remap_funcs_twp.py`, and as a chain of `HalRotate` calls with hand-chosen +signs in `vismach/xyzacb-trsrn-gui.py`. The three copies share no vocabulary +and no sign convention, so "the same as" can be checked only by a person +reading all three. + +This chapter fixes the vocabulary and states the rules, so that two modules +written from the same drawing give the same answers and a consumer can say what +it needs without naming a machine. <> +collects the rules in one place. + +[[sec:frames]] +== Frames + +Four frames, all right-handed. + +Joint space:: + One coordinate per joint, in that joint's own units. This is what the motion + controller commands and what `kinematicsForward()` is given. + +Machine frame:: + Fixed to the machine bed, with the X, Y and Z of + <>. Nothing rotates it. + +Work frame:: + Fixed to the workpiece. `kinematicsForward()` reports in it, and + `kinematicsInverse()` is given it. Where no rotary carries the work it + coincides with the machine frame. Where one does, it turns with the table and + the module undoes that rotation, so that a G-code position keeps naming the + same feature of the part however the table is set. + +Tool frame:: + Fixed to the tool. Its origin is the controlled point of + <>, the tool tip when a tool length + offset is in effect and the tip of the spindle otherwise. Its third axis is + the tool axis, <>. + +The interface calls the work frame "world": the forward and inverse take a +`struct EmcPose *world`, and the motion controller speaks of world mode. That +name is historical and stays in the code. This chapter says work frame, because +the frame is attached to the workpiece and not to the room. + +A pose is always a pair of frames, the moving one and the one it is measured +against, and the two halves of this chapter use different pairs deliberately. + +Positions are measured in the work frame, which is what makes a program +independent of how the table is set. + +Orientations are measured against the machine frame, and there are two of them. +A module reports the tool frame and the work frame separately, each in machine +coordinates. A consumer that wants the tool in workpiece coordinates composes +them: + + tool_in_work = transpose(work) * tool + +A frame written as a matrix is the rotation whose columns are its axes in the +coordinates it is measured against, so the tool axis in workpiece coordinates, +the vector a tilted work plane asks the machine to reach and the one existing +TWP code reads out of `matrix[0,2]`, `matrix[1,2]` and `matrix[2,2]`, is the +third column of that product. + +The pair is reported rather than the product because the product cannot be +taken apart again, and a consumer that has to place both bodies needs each one +against something that does not move. A simulation model draws the workpiece in +one place and the tool in another; given only the product it can recover +neither. Where only the tool turns, the work frame is the identity; where only +the work turns, the tool frame is. A machine that turns both returns a real +pair. + +[[sec:rotation-sense]] +== Rotation Sense + +LinuxCNC states its rotation convention in +<>: + +[quote] +The rotational axes are measured in degrees as wrapped linear axes in which the +direction of positive rotation is counterclockwise when viewed from the +positive end of the corresponding X, Y, or Z-axis. [...] Clockwise or +counterclockwise is from the point of view of the workpiece. + +The second sentence is the one that matters here. The rotation described is +that of the tool relative to the workpiece. Where the rotary carries the tool +that is also the direction the physical axis turns; where it carries the work +the table turns the other way, and the module converts between the two. This is +the convention of ISO 841, which describes all motion as motion of the tool +relative to the workpiece and primes the axes of a machine that moves the work: +a table turning about Z is `+C'` when it produces the tool motion called `+C`. + +So: given a pose whose C value increases, the tool moves counterclockwise about +work Z seen from the workpiece, whichever member physically turns. + +=== conventional-directions + +`trtfuncs.c` and `maxkins.c` carry a `conventional-directions` HAL pin that +selects the sign of the rotary terms, and default it to false, which is the +opposite sense. Existing configurations keep working; new configurations set it +true. + +Leaving it false costs two things. The direction a program runs in depends on a +HAL pin rather than on the G-code. And the rotary values the module reports are +the raw joint values, + +[source,c] +---- +pos->a = joints[JA]; +pos->c = joints[JC]; +---- + +while the translations in the same call were computed with the opposite sign, +so the returned pose does not describe its own orientation. A caller cannot +rebuild the tool frame from `pos->a`, `pos->b` and `pos->c` without separately +knowing how the pin is set. That is the immediate reason the tool frame has to +be an answer from the module rather than something a caller derives from the +pose. + +[[sec:tool-frame]] +== The Tool Frame + +=== The tool axis + +The tool axis is the third axis of the tool frame. It points from the tip +towards the holder, away from the material. + +It is a direction, not a distance, and is unrelated to the tool length: the +length is the scalar the `tool-length` pin carries, and the tool axis is the +direction that length is applied along. + +Where it points for a given joint set is whatever the machine's geometry makes +it. A plain vertical mill has `[0, 0, 1]` in machine coordinates at all times, +and a machine whose spindle is parallel to Z with its rotaries at zero has it +there too, but that is a property of those machines and not a rule. `pumakins` +with its supplied parameters has `[0, 0, -1]` with every joint at zero, and +`genserkins` takes its Denavit-Hartenberg parameters from HAL pins, so for that +module the question has no fixed answer at all. + +[[sec:approach-vector]] +=== Native frames that point the other way + +Robot kinematics name the same line in the opposite sense. ISO 9787 clause 5.3 +places the mechanical interface coordinate system at the centre of the flange, +where the "+Zm axis points perpendicularly away from the mechanical interface", +which runs holder towards tip. The Denavit-Hartenberg approach vector is the +same sense. `pumakins` builds that frame and uses it, reaching the tip by +adding the tool length along the third column: + +[source,c] +---- +hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6; +---- + +Both senses come from a standard for a class of machine, and the tree contains +both. What a module reports is the machine tool sense, tip towards holder, +because that is the direction a tilted work plane commands and what machine Z +already means to a mill operator. + +Turning one sense into the other is not a change of sign. Negating the third +column leaves a matrix of determinant -1, a reflection, which is not a frame +any machine can hold. Reversing the tool axis and staying right-handed takes a +half turn about one of the two transverse axes, and which one is chosen decides +where tool X lands. + +Because it is a rotation in its own right, a module declares it rather than +applying it by hand, as the last argument of `switchkinsRegisterFrames()`. +Shared code applies it and checks once, at load, that it is orthonormal with +determinant +1. `TOOL_FRAME_SPINDLE` is the identity, for a module whose maths +is already in the convention; `TOOL_FRAME_FLANGE` is the half turn a +Denavit-Hartenberg module needs. Keeping it in one place makes it greppable and +stops the next such module quietly choosing the other half turn, which would be +right about the tool axis and wrong about tool X. + +The declaration is fixed when the module is written and nothing changes it at +runtime. A machine whose native frame moved while running would be a machine +whose geometry moves underneath the program. + +=== Tool X + +A tilted work plane commands only where the tool points. Reaching that +direction uses both rotaries of a five-axis machine, and the rotation of the +tool about its own axis is then whatever the chain leaves rather than anything +the program chose. For cutting that does not matter, the cutter being a solid +of revolution. It matters as soon as the tool frame is used as a coordinate +system for programming, which is what `G68.2` does: the operator writes X and Y +moves in the tilted plane and has to know where its X points. + +So the software places it, through a virtual rotation about the tool axis +applied after the physical joints. It is the `pre-rot` pin on the in-tree +kinematics components and `virtual_rot` in the TWP code, one quantity under two +names. A machine with no such pin has no say in the matter: its tool X is +whatever the chain produces, and a consumer that needs a defined one applies +the rotation itself. + +[IMPORTANT] +By default, tool X lies parallel to the machine XY plane. Where the tool axis +is vertical and that leaves tool X free, tool X is machine X. `G68.3 R` rotates +the frame from there. + +This fixes tool X only up to a half turn, two opposite directions both being +horizontal. Where a module has to choose, it takes the one that keeps the frame +continuous with the previous pose. + +=== Deriving the default rotation + +The convention is stated rather than a formula because the formula differs from +machine to machine and follows from the convention. Write the tool orientation +as the product of the primary, secondary and virtual rotations: + + M = Rp(theta_1) * Rs(theta_2) * Rz(tc) + +Tool X is the first column of `M`, so "tool X is horizontal" is the statement +that `M[2][0]` is zero. Solving that for `tc` gives the default. + +For a nutating head of nutation angle `v`, writing `Sv = sin(v)`, +`Cv = cos(v)`, `Ss = sin(theta_2)`, `Cs = cos(theta_2)`, +`s = Cs + Cv*Cv*(1 - Cs)` and `t = Sv*Cv*(1 - Cs)`, the two nutating machines +in the tree have different secondary rotations, and so different bottom rows: + +[cols="1,2,2",options="header"] +|=== +| machine | bottom row of `Rs` | resulting default + +| `xyzacb-trsrn` +| `[-Sv*Ss, t, s]` +| `tc = atan2(Sv*Ss, t)` + +| `xyzbca-trsrn` +| `[t, Sv*Ss, s]` +| `tc = atan2(-t, Sv*Ss)` +|=== + +The two formulas look unrelated and are the same rule. Apply either to the +other machine and the result is a frame whose tool *Y* is horizontal, a quarter +turn from what was wanted, and no test in the tree notices: the tool still +points where it was told to point, and only the meaning of X and Y in the +tilted plane has changed. A module that documents its `Rs` and cites this rule +can be checked. One that documents only its `tc` formula cannot. + +[[sec:consumer-needs]] +== What a Module Reports + +Position, through `kinematicsForward()` and `kinematicsInverse()`, in the work +frame. + +Orientation, through `kinematicsWorkFrame()` and `kinematicsToolFrame()`, each +against the machine frame, as <> describes. The consumers +are tilted work plane handling, tool length compensation along a tilted axis, +previews and simulation models, and probing routines that have to say which way +the stylus faces. + +The Jacobian, relating commanded velocity to joint velocity at a given pose, so +that a feed can be checked against the joint velocity, acceleration and limit +values it will actually demand, and so that proximity to a singularity is a +number rather than a surprise. A module with a closed form can supply it +directly. Otherwise it can be obtained by differencing `kinematicsInverse()` +about the pose, which needs no change to the module at all. + +All of these are functions of the joint values and the module's own geometry. +None needs state carried between calls, and none needs the module to be running +in a realtime thread to be useful: the interesting callers, a limit check +before a move and a preview before a program runs, are not in the servo loop. + +[[sec:writing-a-module]] +== Writing a Module + +Frames:: + Report positions in the work frame, so a rotary that carries the work is + undone in `kinematicsForward()`. Report the work frame and the tool frame + separately, each against the machine frame, so that a consumer placing both + bodies can. + +Signs:: + Positive A, B and C are counterclockwise about work X, Y and Z viewed from + the positive end, describing the motion of the tool relative to the + workpiece. A new module does not offer a pin that reverses this. + +Tool axis:: + Tip towards holder. A module whose maths is written with the + Denavit-Hartenberg approach vector declares the half turn that relates the + two, rather than applying it by hand. + +Tool X:: + The default virtual rotation puts tool X parallel to the machine XY plane. + Derive the value from the module's own rotation matrices, and write those + matrices down in the module. + +Geometry stays in the module:: + Whatever a consumer needs to know about the machine's shape is answered by + the module. A consumer that restates it has taken a copy that nothing keeps + in step, which is the situation this chapter exists to end. + +Mount orientation is not this:: + A tool or holder mount orientation is a different quantity: a right-angle + head, a tool held at an angle, an end effector clocked on its flange. Those + vary from setup to setup and belong with the rest of the tool data, addressed + from the program where the interpreter can see them, and not in HAL alone + where lookahead and preview cannot see them and where they can move + underneath a running program. The tool table and `G43.1` already carry + per-tool A, B and C words, which is the right addressing route, but they + shift the rotary axis reading and configurations depend on that, so a mount + orientation is a new field rather than a reinterpretation of that one. It + wants to be stated as a frame, which can be checked for orthonormality and + determinant, rather than as three angles whose ordering convention is written + down nowhere. + +== References + +* <>, for the axis nomenclature + and the rotation convention this chapter builds on. +* <>, for worked transformations of + the table-rotary and tilting-table configurations. +* <>, for how a machine + presents more than one of these models at once. +* ISO 841, Industrial automation systems and integration, Numerical control of + machines, Coordinate system and motion nomenclature. +* ISO 9787, Robots and robotic devices, Coordinate systems and motion + nomenclatures, clause 5.3, for the flange frame the robot modules follow. From 03eae8d197d2f28a3a0b0a1c76d68e753bfe4b7f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 23 Aug 2026 15:24:15 +1000 Subject: [PATCH 11/58] kinematics: add optional work frame and tool frame entry points kinematicsForward() reports where the controlled point is and nothing about which way anything faces, so a consumer that needs the geometry rebuilds it for itself. Add two entry points a module can answer with instead: int kinematicsToolFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); Each returns the columns of that frame's axes in machine coordinates. Conventions are in the Kinematics Conventions chapter. They are reported separately rather than as the single work-to-tool rotation because the product cannot be taken apart again. A consumer that has to place both bodies, a simulation model or a tracking display, needs each against something that does not move; one that wants the tool in workpiece coordinates, which is what a tilted work plane asks for, composes them with toolFrameInWork(). Composing is a multiply, decomposing is impossible, so the halves are what the module owes the caller. Modules built on switchkins.c export both and dispatch on the current type, returning -1 for a type that has not supplied them. A type registers with switchkinsRegisterFrames() from its switchkinsSetup(); leaving it out costs nothing. Adding it that way rather than as arguments to switchkinsSetup() and switchkinsRegister() keeps both signatures as they are, so no module has to change to build. The tool frame has two live conventions, so a type also declares the rotation relating its own to the one in use, and the dispatch applies it. That is a rotation and not a sign: negating the third column alone leaves determinant -1, a reflection. It is checked once at registration rather than on each call. The work frame needs none of this, having no tool axis to point the wrong way. Identity types answer both the same way whichever module asked for them, so switchkins.c attaches the identity pair to any type whose forward is the identity one, and every switchkins module gains correct frames for its identity type without being touched. Nothing in motion calls either, so no module is obliged to define them and a module outside the tree need not know they exist. --- src/emc/kinematics/kinematics.h | 82 +++++++++++++++++++ src/emc/kinematics/kins_util.c | 136 ++++++++++++++++++++++++++++++++ src/emc/kinematics/switchkins.c | 75 ++++++++++++++++++ src/emc/kinematics/switchkins.h | 15 ++++ 4 files changed, 308 insertions(+) diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 67e6565155d..30da3cb7af7 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -102,6 +102,54 @@ extern int kinematicsHome(struct EmcPose * world, extern KINEMATICS_TYPE kinematicsType(void); +/* These two give the orientation of the tool and of the workpiece for a set + of joint values. Each returns a rotation whose columns are that frame's + axes expressed in MACHINE coordinates, the frame fixed to the bed that + nothing rotates. Note that this is not the frame kinematicsForward() + reports positions in, which is attached to the workpiece; see the + Kinematics Conventions chapter. + + They are reported separately, and not as the single work-to-tool rotation, + because the product cannot be taken apart again. A consumer that has to + place both bodies, a simulation model or a preview, needs each one against + the machine. A consumer that wants the tool in workpiece coordinates, + which is what a tilted work plane asks for, composes them itself: + + tool_in_work = transpose(work) * tool + + The third column of the tool frame is the tool axis: a direction, not to be + confused with the tool length, which is the distance applied along it. It + runs from the tool tip towards the holder. The origin of the tool frame is + the controlled point that kinematicsForward() reports for the same joints. + Where a module applies a virtual rotation about the tool axis, the frame + returned includes it. + + A module whose own maths is in the other sense, which is every module built + on the ISO 9787 flange frame or on Denavit-Hartenberg parameters, does not + fix that up by hand: it declares the rotation relating its frame to the + convention and the shared code applies it. Reversing the tool axis is a + rotation, not a sign. Negating the third column alone gives determinant -1, + a reflection, and which half turn is used decides where tool x ends up. + + A machine that turns only the tool returns the identity for the work frame, + and one that turns only the work returns the identity for the tool frame. + Machines that do both, which is every table-rotary head-rotary mill, return + a non-trivial pair and are the reason for reporting them apart. + + Both are optional. Modules built on switchkins.c export them always and + return -1 for a switchkins type that has not supplied one; other modules + need not export them at all, so a caller resolving them dynamically has to + cope with their absence. + + Return 0 on success, -1 if the frame is not available. */ +extern int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +extern int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + /* parameters for use with switchkins.c */ typedef struct kinematics_parms { char* sparm; // module string parameter passed to kins @@ -157,6 +205,40 @@ extern int identityKinematicsInverse(const struct EmcPose * world, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); +/* joints are axes, so neither frame ever turns */ +extern int identityKinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +extern int identityKinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +/* Rotations relating a module's own frame to the tool frame convention. + TOOL_FRAME_SPINDLE is the identity, for maths already in the convention. + TOOL_FRAME_FLANGE is the half turn about tool x that turns an ISO 9787 + flange frame, whose z points out of the mechanical interface towards the + work, into the convention. */ +extern const PmRotationMatrix TOOL_FRAME_SPINDLE; +extern const PmRotationMatrix TOOL_FRAME_FLANGE; + +/* Post-multiply a module's native frame by the rotation it declared, in + place. Modules built on switchkins.c never call this, the dispatch does it + for them; a standalone module calls it before returning. + Returns 0, or -1 if native is not a proper rotation. */ +extern int toolFrameApplyNative(PmRotationMatrix *rot, + const PmRotationMatrix *native); + +/* out = transpose(work) * tool, the tool frame in workpiece coordinates. + out may alias neither input. */ +extern int toolFrameInWork(const PmRotationMatrix *work, + const PmRotationMatrix *tool, + PmRotationMatrix *out); + +/* True if m is orthonormal with determinant +1, so a frame a machine can + actually hold. Used to check a declared rotation once, at load. */ +extern int toolFrameIsProper(const PmRotationMatrix *m); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); //NOTE: switchable kinematics may require Interp::Synch diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index c82a4a2fc95..1576f7fb505 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -45,6 +45,7 @@ #include #include +#include #include #include #include @@ -364,3 +365,138 @@ int identityKinematicsInverse(const EmcPose * pos, return 0; } // identityKinematicsInverse() + +const PmRotationMatrix TOOL_FRAME_SPINDLE = { + { 1, 0, 0}, // tool x + { 0, 1, 0}, // tool y + { 0, 0, 1} // tool axis +}; + +// half turn about tool x: reverses the tool axis and tool y, keeps tool x, +// and keeps the frame right-handed. Negating the tool axis on its own would +// leave a reflection, which is not a frame any machine can hold. +const PmRotationMatrix TOOL_FRAME_FLANGE = { + { 1, 0, 0}, + { 0, -1, 0}, + { 0, 0, -1} +}; + +int toolFrameIsProper(const PmRotationMatrix *m) +{ + const double c[3][3] = { + { m->x.x, m->y.x, m->z.x }, + { m->x.y, m->y.y, m->z.y }, + { m->x.z, m->y.z, m->z.z } + }; + double det; + int a, b, k; + + for (a = 0; a < 3; a++) { + for (b = a; b < 3; b++) { + double dot = 0; + for (k = 0; k < 3; k++) { dot += c[k][a] * c[k][b]; } + if (fabs(dot - (a == b ? 1.0 : 0.0)) > 1e-9) { return 0; } + } + } + + det = c[0][0] * (c[1][1]*c[2][2] - c[1][2]*c[2][1]) + - c[0][1] * (c[1][0]*c[2][2] - c[1][2]*c[2][0]) + + c[0][2] * (c[1][0]*c[2][1] - c[1][1]*c[2][0]); + + return fabs(det - 1.0) <= 1e-9; +} // toolFrameIsProper() + +int toolFrameApplyNative(PmRotationMatrix *rot, + const PmRotationMatrix *native) +{ + // rot holds the module's own frame, native the rotation relating it to + // the convention, so the answer is rot * native: the declared rotation is + // expressed in the module's frame, not in machine coordinates. + const double r[3][3] = { + { rot->x.x, rot->y.x, rot->z.x }, + { rot->x.y, rot->y.y, rot->z.y }, + { rot->x.z, rot->y.z, rot->z.z } + }; + const double n[3][3] = { + { native->x.x, native->y.x, native->z.x }, + { native->x.y, native->y.y, native->z.y }, + { native->x.z, native->y.z, native->z.z } + }; + double m[3][3]; + int a, b, k; + + if (!toolFrameIsProper(native)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "toolFrameApplyNative: declared rotation is not a proper rotation\n"); + return -1; + } + + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m[a][b] = 0; + for (k = 0; k < 3; k++) { m[a][b] += r[a][k] * n[k][b]; } + } + } + + rot->x.x = m[0][0]; rot->y.x = m[0][1]; rot->z.x = m[0][2]; + rot->x.y = m[1][0]; rot->y.y = m[1][1]; rot->z.y = m[1][2]; + rot->x.z = m[2][0]; rot->y.z = m[2][1]; rot->z.z = m[2][2]; + + return 0; +} // toolFrameApplyNative() + +int toolFrameInWork(const PmRotationMatrix *work, + const PmRotationMatrix *tool, + PmRotationMatrix *out) +{ + // transpose(work) * tool: both are given against the machine, and + // transposing the work frame turns "machine to work" out of "work to + // machine" without a general inverse, because a rotation is orthonormal + const double w[3][3] = { + { work->x.x, work->y.x, work->z.x }, + { work->x.y, work->y.y, work->z.y }, + { work->x.z, work->y.z, work->z.z } + }; + const double t[3][3] = { + { tool->x.x, tool->y.x, tool->z.x }, + { tool->x.y, tool->y.y, tool->z.y }, + { tool->x.z, tool->y.z, tool->z.z } + }; + double m[3][3]; + int a, b, k; + + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m[a][b] = 0; + for (k = 0; k < 3; k++) { m[a][b] += w[k][a] * t[k][b]; } + } + } + + out->x.x = m[0][0]; out->y.x = m[0][1]; out->z.x = m[0][2]; + out->x.y = m[1][0]; out->y.y = m[1][1]; out->z.y = m[1][2]; + out->x.z = m[2][0]; out->y.z = m[2][1]; out->z.z = m[2][2]; + + return 0; +} // toolFrameInWork() + +int identityKinematicsWorkFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)joints; + (void)fflags; + // nothing carries the work, so it stays square with the machine + *rot = TOOL_FRAME_SPINDLE; + return 0; +} // identityKinematicsWorkFrame() + +int identityKinematicsToolFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)joints; + (void)fflags; + // joints are axes, so the tool stays square with the machine + *rot = TOOL_FRAME_SPINDLE; + return 0; +} // identityKinematicsToolFrame() diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index f1393867e35..4f62277a287 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -42,6 +42,9 @@ static kparms kp; // kinematics parms (common all types) static KS ksetups[SWITCHKINS_MAX_TYPES] = {NULL}; static KF kfwds[SWITCHKINS_MAX_TYPES] = {NULL}; static KI kinvs[SWITCHKINS_MAX_TYPES] = {NULL}; +static KT ktools[SWITCHKINS_MAX_TYPES] = {NULL}; +static KT kworks[SWITCHKINS_MAX_TYPES] = {NULL}; +static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; // types provided, counted in rtapi_app_main() once they are all in static int kins_count; @@ -212,6 +215,39 @@ int kinematicsInverse(const EmcPose * pos, return r; } // kinematicsInverse() +int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !ktools[switchkins_type]) { + return -1; // this type does not supply one; not an error + } + r = ktools[switchkins_type](joint, rot, fflags); + if (r) { return r; } + + // the type answers in its own frame; put it in the convention here so + // no module has to get the half turn right for itself + return toolFrameApplyNative(rot, &knative[switchkins_type]); +} // kinematicsToolFrame() + +int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !kworks[switchkins_type]) { + return -1; // this type does not supply one; not an error + } + // no native rotation here: the work frame has no tool axis to point the + // wrong way, so there are not two conventions for it to be caught between + return kworks[switchkins_type](joint, rot, fflags); +} // kinematicsWorkFrame() + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -240,6 +276,32 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) return 0; } // switchkinsRegister() +int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, + const PmRotationMatrix *native) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterFrames: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + // check the declared rotation once here rather than on every call + if (!native || !toolFrameIsProper(native)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterFrames: switchkins-type %d" + " declared a rotation that is not orthonormal with" + " determinant +1\n", ktype); + register_error = 1; + return -1; + } + kworks[ktype] = kwork; + ktools[ktype] = ktool; + knative[ktype] = *native; + return 0; +} // switchkinsRegisterFrames() + //********************************************************************* static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); @@ -251,7 +313,10 @@ EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(switchkinsRegister); +EXPORT_SYMBOL(switchkinsRegisterFrames); MODULE_LICENSE("GPL"); static int comp_id; @@ -280,6 +345,16 @@ int rtapi_app_main(void) if (res) {emsg="switchkinsSetp FAIL"; goto error;} if (register_error) {emsg="switchkinsRegister FAIL"; goto error;} + // an identity type answers the tool frame the same way whichever module + // asked for it, so supply it here rather than in every switchkinsSetup() + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (!ktools[i] && kfwds[i] == identityKinematicsForward) { + kworks[i] = identityKinematicsWorkFrame; + ktools[i] = identityKinematicsToolFrame; + knative[i] = TOOL_FRAME_SPINDLE; + } + } + // the highest type provided by either route sets the count for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 2f9ee530a7c..7815fe2ab39 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -21,6 +21,12 @@ typedef int (*KI)(const struct EmcPose * world, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); +// KinematicsWORKFRAME and KinematicsTOOLFRAME functions +// (optional, see kinematics.h) +typedef int (*KT)(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + // KinematicsSETUP functions typedef int (*KS)(const int comp_id, // halpins const char* coordinates, // module parameter @@ -37,4 +43,13 @@ extern int switchkinsSetup(kparms* ksetup_parms, // called from switchkinsSetup(), once per type it does not provide itself extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); + +// called from switchkinsSetup() for each type that reports its frames; a type +// that does not simply omits the call. Both are given, since a machine has a +// work frame whether or not anything turns it. native is the rotation +// relating the type's own tool frame to the convention, TOOL_FRAME_SPINDLE +// for maths already in it; it is checked once at load and applied by the +// dispatch. +extern int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, + const PmRotationMatrix *native); #endif // } From dd040367b18642568456a70e10c357df5fb774b4 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 23 Aug 2026 15:24:40 +1000 Subject: [PATCH 12/58] trtfuncs: supply the work and tool frames for xyzac and xyzbc Both rotaries carry the work on these machines, so the tool never turns in the machine frame and its frame is the identity. All the rotation is the work's. The forward transform already contains it: the coefficients it applies to a displacement of the X, Y and Z joints are the rotation from machine into work, so the work frame in machine coordinates is their transpose. Checked against the forward transform by central difference over the linear joints at four primary and four secondary angles with both settings of conventional-directions: the composition transpose(work) * tool reproduces the frame to 3e-9, the result is orthonormal with determinant one, and the tool axis is machine z with the rotaries at zero. The check also shows the sign question plainly. With conventional-directions true, A at 90 degrees on an xyzac machine puts the tool axis along -Y in work coordinates, which is +Z turned counterclockwise about +X as the documentation says it should be. With the pin at its default of false the same move puts it along +Y. --- src/emc/kinematics/kinematics.h | 16 ++++++ src/emc/kinematics/trtfuncs.c | 76 +++++++++++++++++++++++++++++ src/emc/kinematics/xyzac-trt-kins.c | 6 +++ src/emc/kinematics/xyzbc-trt-kins.c | 6 +++ 4 files changed, 104 insertions(+) diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 30da3cb7af7..8efeb26c640 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -283,6 +283,14 @@ extern int xyzacKinematicsInverse(const EmcPose * pos, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); +extern int xyzacKinematicsToolFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +extern int xyzacKinematicsWorkFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + extern int xyzbcKinematicsForward(const double *joints, EmcPose * pos, @@ -294,4 +302,12 @@ extern int xyzbcKinematicsInverse(const EmcPose * pos, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); +extern int xyzbcKinematicsToolFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +extern int xyzbcKinematicsWorkFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + //********************************************************************* diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 1a991b0068f..31c9ff1a6de 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -260,6 +260,45 @@ int xyzacKinematicsInverse(const EmcPose * pos, return 0; } // xyzacKinematicsInverse() +int xyzacKinematicsWorkFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + // the forward transform's coefficients for a displacement of the X, Y and + // Z joints are the rotation from machine into work, so the work frame in + // machine coordinates is their transpose, written out directly here + const double a_rad = joints[JA]*TO_RAD; + const double c_rad = joints[JC]*TO_RAD; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + rot->x.x = cos(c_rad); + rot->y.x = con * sin(c_rad); + rot->z.x = 0; + + rot->x.y = - con * sin(c_rad) * cos(a_rad); + rot->y.y = cos(c_rad) * cos(a_rad); + rot->z.y = con * sin(a_rad); + + rot->x.z = sin(c_rad) * sin(a_rad); + rot->y.z = - con * cos(c_rad) * sin(a_rad); + rot->z.z = cos(a_rad); + + return 0; +} // xyzacKinematicsWorkFrame() + +int xyzacKinematicsToolFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)joints; + (void)fflags; + // both rotaries carry the work, so the tool never turns in the machine + *rot = TOOL_FRAME_SPINDLE; + return 0; +} // xyzacKinematicsToolFrame() + int xyzbcKinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -364,3 +403,40 @@ int xyzbcKinematicsInverse(const EmcPose * pos, return 0; } // xyzbcKinematicsInverse() + +int xyzbcKinematicsWorkFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + // see the comment in xyzacKinematicsWorkFrame() + const double b_rad = joints[JB]*TO_RAD; + const double c_rad = joints[JC]*TO_RAD; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + rot->x.x = cos(c_rad) * cos(b_rad); + rot->y.x = con * sin(c_rad) * cos(b_rad); + rot->z.x = - con * sin(b_rad); + + rot->x.y = - con * sin(c_rad); + rot->y.y = cos(c_rad); + rot->z.y = 0; + + rot->x.z = con * cos(c_rad) * sin(b_rad); + rot->y.z = sin(c_rad) * sin(b_rad); + rot->z.z = cos(b_rad); + + return 0; +} // xyzbcKinematicsWorkFrame() + +int xyzbcKinematicsToolFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)joints; + (void)fflags; + // both rotaries carry the work, so the tool never turns in the machine + *rot = TOOL_FRAME_SPINDLE; + return 0; +} // xyzbcKinematicsToolFrame() diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 47655ec0f14..504f177e9ee 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -38,11 +38,17 @@ int switchkinsSetup(kparms* kp, *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc *kfwd1 = xyzacKinematicsForward; *kinv1 = xyzacKinematicsInverse; + switchkinsRegisterFrames(1, xyzacKinematicsWorkFrame, + xyzacKinematicsToolFrame, + &TOOL_FRAME_SPINDLE); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc *kfwd0 = xyzacKinematicsForward; *kinv0 = xyzacKinematicsInverse; + switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, + xyzacKinematicsToolFrame, + &TOOL_FRAME_SPINDLE); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index aa1289baf28..6915099c832 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -38,11 +38,17 @@ int switchkinsSetup(kparms* kp, *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc *kfwd1 = xyzbcKinematicsForward; *kinv1 = xyzbcKinematicsInverse; + switchkinsRegisterFrames(1, xyzbcKinematicsWorkFrame, + xyzbcKinematicsToolFrame, + &TOOL_FRAME_SPINDLE); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc *kfwd0 = xyzbcKinematicsForward; *kinv0 = xyzbcKinematicsInverse; + switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, + xyzbcKinematicsToolFrame, + &TOOL_FRAME_SPINDLE); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; From bdb94f3344e954c972da85bd99847efcebd4987c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 23 Aug 2026 15:29:33 +1000 Subject: [PATCH 13/58] trivkins: supply the work and tool frames trivkins does not build on switchkins.c, so it does not pick up the identity frames the way a switchkins identity type does. Hand them through, since it is the kinematics most machines run and a caller that has to special-case the commonest module has not gained much. --- src/emc/kinematics/trivkins.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 3ea56b49aa8..4b3685dc6d6 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -38,6 +38,20 @@ int kinematicsInverse(const EmcPose * pos, return identityKinematicsInverse(pos, joints, iflags, fflags); } +int kinematicsToolFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + return identityKinematicsToolFrame(joints, rot, fflags); +} + +int kinematicsWorkFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + return identityKinematicsWorkFrame(joints, rot, fflags); +} + static KINEMATICS_TYPE ktype = -1; KINEMATICS_TYPE kinematicsType() @@ -56,6 +70,8 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsWorkFrame); MODULE_LICENSE("GPL"); static int comp_id; From e57a08536338bdce1e489db6a6574441d014589c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 23 Aug 2026 15:29:34 +1000 Subject: [PATCH 14/58] xyzacb_trsrn, xyzbca_trsrn: supply the work and tool frames These are the machines the split is for: a rotary carries the work and two more carry the tool, so neither frame is the identity and neither can be recovered from their product. The work frame is the table rotation, written in machine coordinates. The tool frame is the primary rotation about z times the nutating secondary, written as two matrices and multiplied rather than expanded, so it can be read against the matrices in the config's remap_funcs_twp.py. Identity kinematics leaves both square with the machine, and so does tool kinematics: there the world axes are the tool axes by construction, which is what makes a G1 Z move run along the tool, so there is no machine-relative frame to report. Checked against the forward transform of each module at 27 poses. The coefficients the forward applies to a displacement of the linear joints are the transpose of the work frame alone, as they should be, since turning the head does not move the tool tip when a linear joint moves. The tool axis, recovered separately as the direction the tip retreats along when the tool gets longer, matches the third column of transpose(work) * tool. For tool kinematics the same coefficients come out as the transpose of the whole chain including the virtual rotation, which is the statement that the world frame is the tool frame. Agreement is to 3e-9 throughout. --- src/hal/components/xyzacb_trsrn.comp | 84 ++++++++++++++++++++++++++++ src/hal/components/xyzbca_trsrn.comp | 84 ++++++++++++++++++++++++++++ 2 files changed, 168 insertions(+) diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index dfbe4466ace..098af8a7aa2 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -90,6 +90,8 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsWorkFrame); static rtapi_u32 switchkins_type; @@ -304,6 +306,88 @@ int kinematicsForward(const double *j, return 0; } // kinematicsForward() +// These modules do not link kins_util.c, so they cannot reach the shared +// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. +static void frame_square_with_machine(PmRotationMatrix *rot) +{ + rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; + rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; +} + +int kinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = hal_get_real(haldata->nut_angle); // degrees + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Ss = sin(j[4]*TO_RAD); + double Cs = cos(j[4]*TO_RAD); + double Sp = sin(j[5]*TO_RAD); + double Cp = cos(j[5]*TO_RAD); + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int a, b, k; + + // identity kinematics, and tool kinematics where the world axes are the + // tool axes by construction, both leave the tool square with the machine + if (switchkins_type != 1) { + frame_square_with_machine(rot); + return 0; + } + + // the primary joint turns the head about z + const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; + + // the nutating secondary joint + const double Rs[3][3] = {{Cs, -Cv*Ss, Sv*Ss}, + {Cv*Ss, r, t}, + {-Sv*Ss, t, s}}; + + double M[3][3]; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + M[a][b] = 0; + for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } + } + } + + rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; + rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; + rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; + + return 0; +} // kinematicsToolFrame() + +int kinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double Sw = sin(j[3]*TO_RAD); + double Cw = cos(j[3]*TO_RAD); + + // in tool kinematics the world axes are the tool axes, so the work is not + // being reported against the machine and there is nothing to turn + if (switchkins_type != 1) { + frame_square_with_machine(rot); + return 0; + } + + // the A joint carries the work: its frame in machine coordinates + // is a rotation about x by the joint value + const double W[3][3] = {{1, 0, 0}, {0, Cw, Sw}, {0, -Sw, Cw}}; + + rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; + rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; + rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; + + return 0; +} // kinematicsWorkFrame() + int kinematicsInverse(const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index b8f451c17f9..12e40344125 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -90,6 +90,8 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsWorkFrame); static rtapi_u32 switchkins_type; @@ -309,6 +311,88 @@ int kinematicsForward(const double *j, return 0; } // kinematicsForward() +// These modules do not link kins_util.c, so they cannot reach the shared +// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. +static void frame_square_with_machine(PmRotationMatrix *rot) +{ + rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; + rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; +} + +int kinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = hal_get_real(haldata->nut_angle); // degrees + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Ss = sin(j[3]*TO_RAD); + double Cs = cos(j[3]*TO_RAD); + double Sp = sin(j[5]*TO_RAD); + double Cp = cos(j[5]*TO_RAD); + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int a, b, k; + + // identity kinematics, and tool kinematics where the world axes are the + // tool axes by construction, both leave the tool square with the machine + if (switchkins_type != 1) { + frame_square_with_machine(rot); + return 0; + } + + // the primary joint turns the head about z + const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; + + // the nutating secondary joint + const double Rs[3][3] = {{r, -Cv*Ss, t}, + {Cv*Ss, Cs, -Sv*Ss}, + {t, Sv*Ss, s}}; + + double M[3][3]; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + M[a][b] = 0; + for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } + } + } + + rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; + rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; + rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; + + return 0; +} // kinematicsToolFrame() + +int kinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double Sw = sin(j[4]*TO_RAD); + double Cw = cos(j[4]*TO_RAD); + + // in tool kinematics the world axes are the tool axes, so the work is not + // being reported against the machine and there is nothing to turn + if (switchkins_type != 1) { + frame_square_with_machine(rot); + return 0; + } + + // the B joint carries the work: its frame in machine coordinates + // is a rotation about y by the joint value + const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}}; + + rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; + rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; + rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; + + return 0; +} // kinematicsWorkFrame() + int kinematicsInverse(const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, From 3098d6e214709b70c2d3684db578f55d6b2246af Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 23 Aug 2026 17:09:31 +1000 Subject: [PATCH 15/58] pumakins: supply the work and tool frames The arm carries the tool and nothing carries the work, so the work frame is the identity and this is the first module whose own tool maths is not in the convention. pumakins builds the ISO 9787 mechanical interface frame, whose z points perpendicularly away from the flange, and it relies on that: it reaches the tool tip by adding PUMA_D6 along the third column. So it answers in its own frame and declares TOOL_FRAME_FLANGE, and switchkins turns it into the convention. Nothing in the module itself flips a sign. Lift the rotation out of the forward kinematics into pumaFlangeRotation() rather than writing it twice, which is the whole point: a second copy of a machine's geometry that has to be kept in step by hand is the thing this work exists to remove. The block moves verbatim and the forward kinematics loses the locals that went with it. At every joint zero the module's own frame is diag(1, -1, -1), a half turn about x, so after the declared half turn it reports the identity: tool axis [0, 0, 1], tool x [1, 0, 0]. A puma at zero and a vertical mill at zero give the same answer, which is right, because both have the tool pointing down at the work. --- src/emc/kinematics/pumakins.c | 69 +++++++++++++++++++++++++++++++---- 1 file changed, 61 insertions(+), 8 deletions(-) diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index f055e73a502..5aaa4066193 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -29,21 +29,17 @@ struct haldata { hal_real_t a2, a3, d3, d4, d6; } *haldata = NULL; -static int pumaKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +/* The flange orientation for a joint set: the ISO 9787 mechanical interface + frame, whose z points out of the interface towards the work. Shared by the + forward kinematics and the tool frame so the two cannot drift apart. */ +static void pumaFlangeRotation(const double * joint, PmRotationMatrix * rot) { - (void)fflags; double s1, s2, s3, s4, s5, s6; double c1, c2, c3, c4, c5, c6; double s23; double c23; double t1, t2, t3, t4, t5; - double sumSq, k; PmHomogeneous hom; - PmPose worldPose; - PmRpy rpy; /* Calculate sin of joints for future use */ s1 = sin(joint[0]*PM_PI/180); @@ -99,6 +95,37 @@ static int pumaKinematicsForward(const double * joint, hom.rot.z.y = -s1 * t1 + c1 * s4 * s5; hom.rot.z.z = s23 * c4 * s5 - c23 * c5; + *rot = hom.rot; +} // pumaFlangeRotation() + +static int pumaKinematicsForward(const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + double s1, s2, s3; + double c1, c2, c3; + double s23; + double c23; + double t1, t2; + double sumSq, k; + PmHomogeneous hom; + PmPose worldPose; + PmRpy rpy; + + pumaFlangeRotation(joint, &hom.rot); + + /* Calculate sin and cos of joints for the position vector */ + s1 = sin(joint[0]*PM_PI/180); + s2 = sin(joint[1]*PM_PI/180); + s3 = sin(joint[2]*PM_PI/180); + c1 = cos(joint[0]*PM_PI/180); + c2 = cos(joint[1]*PM_PI/180); + c3 = cos(joint[2]*PM_PI/180); + s23 = c2 * s3 + s2 * c3; + c23 = c2 * c3 - s2 * s3; + rtapi_real PUMA_A2 = hal_get_real(haldata->a2); rtapi_real PUMA_A3 = hal_get_real(haldata->a3); rtapi_real PUMA_D3 = hal_get_real(haldata->d3); @@ -174,6 +201,27 @@ static int pumaKinematicsForward(const double * joint, return 0; } +static int pumaKinematicsToolFrame(const double * joint, + PmRotationMatrix * rot, + const KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)fflags; + // answers in the flange frame; switchkins applies the declared half turn + pumaFlangeRotation(joint, rot); + return 0; +} // pumaKinematicsToolFrame() + +static int pumaKinematicsWorkFrame(const double * joint, + PmRotationMatrix * rot, + const KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)joint; + (void)fflags; + // the arm carries the tool and nothing carries the work + *rot = TOOL_FRAME_SPINDLE; + return 0; +} // pumaKinematicsWorkFrame() + static int pumaKinematicsInverse(const EmcPose * world, double * joint, const KINEMATICS_INVERSE_FLAGS * iflags, @@ -371,6 +419,11 @@ int switchkinsSetup(kparms* kp, *kset0 = pumaKinematicsSetup; *kfwd0 = pumaKinematicsForward; *kinv0 = pumaKinematicsInverse; + // the maths is the ISO 9787 flange frame, so the tool axis it produces + // runs holder towards tip, the opposite of the convention + switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, + pumaKinematicsToolFrame, + &TOOL_FRAME_FLANGE); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; From 6871bdb58e21fa93ec6552e8fa88d20acfc74040 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 23 Aug 2026 17:35:00 +1000 Subject: [PATCH 16/58] tests: cover the frame helpers Pins down the two properties the chapter is about. Relating one tool axis convention to the other is a rotation, not a change of sign: a negated third column is refused because it is a reflection, the declared rotation post-multiplies so it is read in the module's own frame, the half turn keeps tool x and reverses the other two, and applying it twice is the identity. Also checks the pumakins zero pose, whose own frame is a half turn about x, ends up as the identity after the declaration it makes. And composing the two reported frames means transposing the work one: toolFrameInWork() leaves the tool alone when nothing turns the work, gives a proper rotation, and composes a work frame with its own inverse back to the identity. Verified by mutation rather than by passing: making TOOL_FRAME_FLANGE negate only the tool axis fails nine checks, reversing the multiplication order fails four, and dropping the transpose in toolFrameInWork() fails two. Built the way tests/blendmath builds, compiling the source under test directly with the rest garbage-collected by the linker. --- tests/tool-frame/checkresult | 2 + tests/tool-frame/skip | 4 + tests/tool-frame/test.sh | 17 ++++ tests/tool-frame/test_tool_frame.c | 158 +++++++++++++++++++++++++++++ 4 files changed, 181 insertions(+) create mode 100755 tests/tool-frame/checkresult create mode 100755 tests/tool-frame/skip create mode 100755 tests/tool-frame/test.sh create mode 100644 tests/tool-frame/test_tool_frame.c diff --git a/tests/tool-frame/checkresult b/tests/tool-frame/checkresult new file mode 100755 index 00000000000..722c4557b62 --- /dev/null +++ b/tests/tool-frame/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +grep -q "all tool frame checks passed" "$1" && ! grep -q "FAIL" "$1" diff --git a/tests/tool-frame/skip b/tests/tool-frame/skip new file mode 100755 index 00000000000..ee99160224a --- /dev/null +++ b/tests/tool-frame/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# This test compiles kins_util.c from the source tree, which is only +# available in run-in-place builds. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/tool-frame/test.sh b/tests/tool-frame/test.sh new file mode 100755 index 00000000000..2dae535797b --- /dev/null +++ b/tests/tool-frame/test.sh @@ -0,0 +1,17 @@ +#!/bin/bash +set -e + +# RIP layout: $HEADERS is $TOPDIR/include +TOPDIR=$(dirname "$HEADERS") + +# kins_util.c holds the shared kinematics helpers. Only the tool frame ones +# are exercised here, so build with function sections and let the linker drop +# the rest rather than dragging in the HAL pin setup the others need. +gcc -O2 -Wall -ffunction-sections -fdata-sections -DULAPI \ + -I"$HEADERS" -I"$TOPDIR/src" -I"$TOPDIR/src/emc" \ + -o test_tool_frame test_tool_frame.c "$TOPDIR/src/emc/kinematics/kins_util.c" \ + -L"$LIBDIR" -Wl,-rpath,"$LIBDIR" -Wl,--gc-sections \ + -lposemath -llinuxcnchal -lm + +./test_tool_frame +rm -f test_tool_frame diff --git a/tests/tool-frame/test_tool_frame.c b/tests/tool-frame/test_tool_frame.c new file mode 100644 index 00000000000..dde817ae554 --- /dev/null +++ b/tests/tool-frame/test_tool_frame.c @@ -0,0 +1,158 @@ +/* Unit tests for the tool frame helpers in kins_util.c. + * + * The property worth pinning down is that relating one tool axis convention + * to the other is a rotation and not a change of sign: negating the third + * column on its own leaves a reflection, which is not a frame any machine can + * hold, and it silently loses tool x as well. + */ +#include +#include +#include + +#include "emcpos.h" +#include "kinematics.h" + +static int failures; + +static void check(int ok, const char *what) +{ + if (!ok) { printf("FAIL: %s\n", what); failures++; } +} + +static PmRotationMatrix mat(double xx, double yx, double zx, + double xy, double yy, double zy, + double xz, double yz, double zz) +{ + /* written out in the layout it prints in, so the literal below reads as + the matrix it is: columns are tool x, tool y, tool axis */ + PmRotationMatrix m; + m.x.x = xx; m.y.x = yx; m.z.x = zx; + m.x.y = xy; m.y.y = yy; m.z.y = zy; + m.x.z = xz; m.y.z = yz; m.z.z = zz; + return m; +} + +static int same(const PmCartesian *a, double x, double y, double z) +{ + return fabs(a->x - x) < 1e-12 + && fabs(a->y - y) < 1e-12 + && fabs(a->z - z) < 1e-12; +} + +/* rotation by 40 degrees about z then 25 about y, an arbitrary proper + rotation with no zeros to hide a transposition */ +static PmRotationMatrix arbitrary(void) +{ + const double a = 40.0 * M_PI / 180.0, b = 25.0 * M_PI / 180.0; + const double ca = cos(a), sa = sin(a), cb = cos(b), sb = sin(b); + return mat( ca*cb, -sa, ca*sb, + sa*cb, ca, sa*sb, + -sb, 0, cb); +} + +int main(void) +{ + PmRotationMatrix m, r; + + /* the supplied constants are usable as declarations */ + check(toolFrameIsProper(&TOOL_FRAME_SPINDLE), "TOOL_FRAME_SPINDLE is proper"); + check(toolFrameIsProper(&TOOL_FRAME_FLANGE), "TOOL_FRAME_FLANGE is proper"); + + /* TOOL_FRAME_FLANGE is a half turn about tool x */ + check(same(&TOOL_FRAME_FLANGE.x, 1, 0, 0), "flange keeps tool x"); + check(same(&TOOL_FRAME_FLANGE.y, 0, -1, 0), "flange reverses tool y"); + check(same(&TOOL_FRAME_FLANGE.z, 0, 0, -1), "flange reverses the tool axis"); + + /* the mistake this exists to prevent: negating the tool axis alone is a + reflection, and toolFrameIsProper has to reject it */ + m = TOOL_FRAME_SPINDLE; + m.z.x = -m.z.x; m.z.y = -m.z.y; m.z.z = -m.z.z; + check(!toolFrameIsProper(&m), "a negated third column is rejected"); + + /* and so are the other ways of not being a rotation */ + m = TOOL_FRAME_SPINDLE; m.x.x = 2.0; + check(!toolFrameIsProper(&m), "a scaled column is rejected"); + m = TOOL_FRAME_SPINDLE; m.y.x = 0.5; + check(!toolFrameIsProper(&m), "non-orthogonal columns are rejected"); + + /* applying a declared rotation */ + r = arbitrary(); + m = r; + check(toolFrameApplyNative(&m, &TOOL_FRAME_SPINDLE) == 0, "identity applies"); + check(memcmp(&m, &r, sizeof m) == 0, "identity changes nothing"); + + m = r; + check(toolFrameApplyNative(&m, &TOOL_FRAME_FLANGE) == 0, "flange applies"); + check(toolFrameIsProper(&m), "the result is still a proper rotation"); + check(same(&m.x, r.x.x, r.x.y, r.x.z), "tool x survives the half turn"); + check(same(&m.z, -r.z.x, -r.z.y, -r.z.z), "the tool axis is reversed"); + check(same(&m.y, -r.y.x, -r.y.y, -r.y.z), "tool y is reversed with it"); + + /* the half turn is its own inverse */ + check(toolFrameApplyNative(&m, &TOOL_FRAME_FLANGE) == 0, "flange applies again"); + check(memcmp(&m, &r, sizeof m) == 0, "twice is the identity"); + + /* the declared rotation is in the module's frame, so it post-multiplies. + pre-multiplying would give a different answer for a non-commuting pair, + which is what this catches. */ + m = r; + toolFrameApplyNative(&m, &TOOL_FRAME_FLANGE); + check(fabs(m.y.x - (-r.y.x)) < 1e-12, "post-multiplied, not pre-multiplied"); + + /* an improper declaration is refused rather than applied */ + m = r; + r.z.x = -r.z.x; r.z.y = -r.z.y; r.z.z = -r.z.z; /* reuse r as a bad native */ + check(toolFrameApplyNative(&m, &r) == -1, "an improper declaration is refused"); + + /* pumakins' own frame at every joint zero is a half turn about x, so the + declaration it makes turns it into the identity: the same answer a + vertical mill gives, which is right, because both point at the work */ + m = mat(1, 0, 0, + 0, -1, 0, + 0, 0, -1); + check(toolFrameIsProper(&m), "the puma zero pose frame is proper"); + check(toolFrameApplyNative(&m, &TOOL_FRAME_FLANGE) == 0, "puma declaration applies"); + check(same(&m.x, 1, 0, 0) && same(&m.y, 0, 1, 0) && same(&m.z, 0, 0, 1), + "a puma at zero reports the same frame as a vertical mill"); + + /* the two frames are reported against the machine and composed by the + caller; the product is what a tilted work plane wants, and it is the + thing that cannot be taken apart again, which is why it is not what + the module returns */ + { + PmRotationMatrix work, tool, in_work, back; + + /* nothing turns the work: the tool in work coordinates is the tool */ + work = TOOL_FRAME_SPINDLE; + tool = arbitrary(); + toolFrameInWork(&work, &tool, &in_work); + check(memcmp(&in_work, &tool, sizeof in_work) == 0, + "identity work frame leaves the tool frame alone"); + + /* nothing turns the tool: the tool in work coordinates is the inverse + of the work rotation, so composing it back gives the identity */ + work = arbitrary(); + tool = TOOL_FRAME_SPINDLE; + toolFrameInWork(&work, &tool, &in_work); + check(toolFrameIsProper(&in_work), "the composition is a proper rotation"); + toolFrameInWork(&in_work, &TOOL_FRAME_SPINDLE, &back); + toolFrameInWork(&work, &back, &in_work); + check(same(&in_work.x, 1, 0, 0) && same(&in_work.y, 0, 1, 0) + && same(&in_work.z, 0, 0, 1), + "work composed with its own inverse is the identity"); + + /* both turn, which is the case the split exists for: the work frame + must be transposed, not just multiplied in */ + work = arbitrary(); + tool = TOOL_FRAME_FLANGE; + toolFrameInWork(&work, &tool, &in_work); + check(toolFrameIsProper(&in_work), "a mixed rotation composes properly"); + check(fabs(in_work.z.x - (work.x.x*tool.z.x + work.x.y*tool.z.y + + work.x.z*tool.z.z)) < 1e-12, + "the work frame is transposed, not applied directly"); + } + + if (failures) { printf("%d failure(s)\n", failures); return 1; } + printf("all tool frame checks passed\n"); + return 0; +} From cb6a65225cf6c09fafb04e0878a5246d02167630 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 24 Aug 2026 14:18:03 +1000 Subject: [PATCH 17/58] kinematics: answer which joints point the tool somewhere The frames say where the tool points for a set of joint values. The question a tilted work plane actually asks is the other way round: which joint values point it at the orientation the program wants. Today that is answered outside the kinematics, in per-machine trigonometry in the TWP remap, once per machine and once per pair of rotary letters, which is how the copy in the tree came to check the primary angle against the secondary joint's limits. kinematicsToolFrameInverse() asks the module instead. It reports every set of joint values that reaches the orientation, and nothing else: no joint limits and no preference between the answers, because the caller knows the limits and knows whether the operator asked for the shortest move or for one direction of rotation only. A module that reports its frames needs to supply nothing, the generic search in kins_util.c drives them; a module with a closed form registers it and that is used instead. The request is a tool axis and optionally a tool x as well, and asking for tool x does not require a joint that can reach it. A five axis machine spends both rotaries on the tool axis and the turn about that axis is not a joint, it is the virtual rotation, so the answer comes back in two parts: the poses that reach the axis, and the turn that places tool x, which is zero where a third orientation joint did the job instead. The caller writes one path and the kind of machine becomes a number rather than a branch. That is what the controls do, a Heidenhain PLANE VECTOR block carrying the normal and the base vector together and a Fanuc G68.2 defining where the plane's X points; neither refuses a program for naming both. Where a request still leaves the machine free, at a singular pose or on a machine with a spare orientation joint, one point of the family is reported, the one nearest the seed, with the number of free directions alongside. Returning samples of a curve as though they were alternatives would be worse than saying so. Three details in the search are not incidental. The damping is adaptive, which is what keeps a rank deficient pose from turning finite difference noise into a step of thousands of degrees. The Jacobian is central differenced, because a one sided error is first order in the step and shows up as a spurious singular value, which is exactly what the rank test must not see. And the joint unit is discovered by adding a whole turn and asking whether the frame came back, because every module in the tree takes degrees but the interface does not say so. The unit test drives it through a table rotary machine, a nutating head and a machine with both, and checks the nutating answers against the closed form the remap uses. --- docs/src/motion/kinematics-conventions.adoc | 80 +++ src/emc/kinematics/kinematics.h | 76 +++ src/emc/kinematics/kins_util.c | 517 ++++++++++++++++++++ src/emc/kinematics/switchkins.c | 49 ++ src/emc/kinematics/switchkins.h | 14 + tests/tool-frame/test_tool_frame.c | 411 ++++++++++++++++ 6 files changed, 1147 insertions(+) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 0520eaeae28..6d30f7ed630 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -264,6 +264,11 @@ are tilted work plane handling, tool length compensation along a tilted axis, previews and simulation models, and probing routines that have to say which way the stylus faces. +The joint values that reach a requested orientation, through +`kinematicsToolFrameInverse()`, which is the inverse of the pair above and the +question a tilted work plane asks when it has to orient the machine. +<> says what it answers. + The Jacobian, relating commanded velocity to joint velocity at a given pose, so that a feed can be checked against the joint velocity, acceleration and limit values it will actually demand, and so that proximity to a singularity is a @@ -276,6 +281,76 @@ None needs state carried between calls, and none needs the module to be running in a realtime thread to be useful: the interesting callers, a limit check before a move and a preview before a program runs, are not in the servo loop. +[[sec:orientation-inverse]] +== The Orientation Inverse + +Pointing the tool somewhere is the question a tilted work plane asks on every +orienting move, and the question a program written as tool vectors asks on +every block. It is the inverse of the tool frame: not where the tool points +for these joints, but which joints point it there. + +It has more than one answer. A five-axis machine reaches a given tool axis two +ways, one with the secondary rotary positive and one with it negative, and +which of the two is wanted depends on the joint limits and on what the operator +asked for, shortest move or one direction of rotation only. So the module +reports every set of joint values that reaches the orientation and stops there. +It does not apply the joint limits and it does not rank the answers, because +neither is geometry: a module that picked for the caller would be picking with +less to go on than the caller has. + +The request is a tool axis, and optionally a tool x as well. The two have to be +at right angles, being two axes of one frame. + +Asking for tool x does not require a joint that can reach it. A five-axis +machine spends both rotaries on the tool axis, and the turn about that axis is +not a joint at all: it is the virtual rotation of <>. So the answer comes back in two parts. Where the joints can place tool +x, on a machine with a third orientation joint, they do, and the reported turn +is zero. Where they cannot, the joints reach the axis and the reported turn +finishes the job. The caller writes one path either way, and which kind of +machine it has is a number that happens to be zero rather than a branch. + +This is what the controls do. A Heidenhain `PLANE VECTOR` block carries the +normal and the base vector together, the normal reached by the rotaries and the +base vector applied as a rotation of the coordinate system; `SEQ` then picks +between the rotary solutions. A Fanuc `G68.2` defines the plane including where +its X points, and `G53.1` moves the rotaries to align the tool axis. Siemens +draws the same line from the other side: the turn of the tool about itself is +`THETA`, and it exists only where a third rotary axis does. None of them +refuses a program for naming both directions on a five-axis machine, and +neither should this. + +Some requests still do not pin the machine down. A five-axis machine asked to +point the tool along the axis its primary rotary turns about can hold any +primary angle; a machine with three orientation joints asked only for a tool +axis has a whole curve of solutions. In both cases the answer is a continuum, +so one point of it is reported, the one nearest where the machine already is, +along with the number of directions left free. A caller that hands back a list +of samples from a curve as though they were alternatives is telling the +operator something false. + +=== What a module has to supply + +Nothing, if it already reports its frames. The shared code answers the question +by searching: it finds which joints move `transpose(work) * tool`, and solves +for them. That is the whole reason the frames are worth reporting. A module +that supplies them gets the inverse without deriving anything. + +A module with a closed form registers it and that is used instead. It is faster +than a search and it knows its own degenerate poses without having to discover +them. The nutating heads are the case in point: for those, + + cos(secondary) = (Kzz - Cv^2) / (1 - Cv^2) + +with `Cv = cos(v)` for a nutation angle `v` and `Kzz` the z component of the +requested tool axis, and the primary follows from the other two components. +Both roots of the arc cosine are solutions, which is where the pair of answers +comes from. + +The search is not a realtime routine. How long it takes depends on the machine +and on the request, and the callers that want it, orienting a tilted work plane +and previewing a program, are not in the servo loop. + [[sec:writing-a-module]] == Writing a Module @@ -300,6 +375,11 @@ Tool X:: Derive the value from the module's own rotation matrices, and write those matrices down in the module. +Orientation inverse:: + A module that reports its frames gets it from the shared search and needs to + do nothing. Register a closed form only where one exists, and where it does, + say which poses it treats as degenerate. + Geometry stays in the module:: Whatever a consumer needs to know about the machine's shape is answered by the module. A consumer that restates it has taken a copy that nothing keeps diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 8efeb26c640..e6c459c3179 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -239,6 +239,82 @@ extern int toolFrameInWork(const PmRotationMatrix *work, actually hold. Used to check a declared rotation once, at load. */ extern int toolFrameIsProper(const PmRotationMatrix *m); +/* The inverse of kinematicsToolFrame(): which joint values point the tool + along a requested direction. This is the question a tilted work plane asks + when it has to orient the machine, and the one vector format G-code asks + for every block. + + axis_in_work is the wanted tool axis and x_in_work the wanted tool x, both + in workpiece coordinates, both in the sense of transpose(work) * tool. + x_in_work may be NULL, which leaves the spin about the tool free. Where it + is given, the two have to be at right angles, being two axes of one frame. + + Asking for tool x does not require a joint that can reach it. A five axis + machine spends both rotaries on the tool axis, and the turn about that axis + is not a joint at all: it is the virtual rotation, the pre-rot pin on the + in-tree components. So where the joints can place tool x, on a machine with + a third orientation joint, they do and tool_spin comes back zero; where they + cannot, the joints reach the axis and tool_spin carries the turn about it + that finishes the job, in radians, in the sense of the virtual rotation. + Either way the caller writes one path, and which kind of machine it has is a + number that happens to be zero rather than a branch. tool_spin may be NULL, + but then a request for tool x that the joints cannot reach has nowhere to + put its answer and reports no solutions. + + seed is a full set of joint values, normally where the machine is now. The + joints that do not affect the tool orientation are copied from it, and it + breaks the tie where a machine has more orientation joints than the request + constrains. + + solutions receives max_solutions complete sets of joint values, one after + another, each num_joints long. free_directions, if not NULL, receives one + entry per solution: 0 where the joints are pinned down, and n where the + solution is one point of an n dimensional family, which happens at a + singular pose and on a machine with a spare orientation joint. In that case + one representative is reported, the one nearest the seed, because the answer + is a continuum and a list of samples from it would be arbitrary. + + Joint limits are not applied and no solution is preferred over another: the + module answers what the geometry permits, and the caller picks by whatever + rule it works to, shortest move or positive rotation only or whatever else. + + Returns the number of solutions, 0 if the orientation cannot be reached, or + -1 if the module cannot answer. + + This is not a realtime routine. It searches, and how long it takes depends + on the machine and the request. */ +#define TOOL_FRAME_MAX_SOLUTIONS 8 +#define TOOL_FRAME_MAX_FREE 4 + +extern int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin); + +/* The generic implementation of the above, driven by a module's own frame + functions, so that a module gets it for free once it supplies them. A + module with a closed form registers that instead: it is faster, and it + knows its own degenerate poses without having to find them. + + num_joints is the length of seed and of each row of solutions. */ +typedef int (*kinsFrameFunc)(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +extern int toolFrameSolve(kinsFrameFunc work, + kinsFrameFunc tool, + int num_joints, + const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); //NOTE: switchable kinematics may require Interp::Synch diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 1576f7fb505..ee9d6593c1d 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -500,3 +500,520 @@ int identityKinematicsToolFrame(const double *joints, *rot = TOOL_FRAME_SPINDLE; return 0; } // identityKinematicsToolFrame() + +//---------------------------------------------------------------------- +// toolFrameSolve() +// +// The inverse of the tool orientation, built on nothing but a module's own +// work and tool frame functions, so that supplying those is enough and no +// module has to hand-derive a formula. +// +// The problem is small: the only joints that can turn the tool are rotary +// ones, there are rarely more than three of them, and the orientation is a +// function of those joints alone. So the routine finds which joints move +// transpose(work) * tool, and solves for them by damped least squares from a +// spread of starting points, keeping the roots that are distinct. +// +// Three things are worth naming because they are what the naive version gets +// wrong. +// +// The damping is adaptive. At a singular pose the Jacobian loses rank, and a +// fixed small damping turns the noise in the near-null direction into a step +// of thousands of degrees. Raising the damping when a step fails and lowering +// it when one succeeds is what keeps those poses solvable at all. +// +// The Jacobian is taken with central differences. A one sided difference has +// an error of the same order as the step, and it appears as a spurious small +// singular value, which is exactly what the rank test must not see. +// +// The joint unit is discovered rather than assumed. Every module in the tree +// takes rotary joints in degrees, but the interface does not say so, and the +// search has to cover exactly one turn. Adding a whole turn and asking +// whether the frame came back settles it, and rescaling into a unit where one +// turn is 2*pi makes the damping and the step limits the same on any module. +//---------------------------------------------------------------------- + +#define TFS_MAX_RES 6 // three for the tool axis, three for tool x +#define TFS_ITERS 60 +#define TFS_FD_STEP 1e-6 // internal radians +#define TFS_MOVED_TOL 1e-9 // frame difference that counts as movement +#define TFS_RANK_TOL 1e-4 // a direction worth less than this is free +#define TFS_SOLVED 1e-18 // sum of squared residuals +#define TFS_STEP_LIMIT 0.4 // internal radians per iteration + +typedef struct { + kinsFrameFunc work; + kinsFrameFunc tool; + int num_joints; + const double *seed; + int nfree; + int free[TOOL_FRAME_MAX_FREE]; + double scale[TOOL_FRAME_MAX_FREE]; // joint units per internal radian + int nres; + double want[TFS_MAX_RES]; + double joint[EMCMOT_MAX_JOINTS]; // scratch, rebuilt on every call +} tfs_ctx; + +// transpose(work) * tool at a joint set, as the columns the request names +static int tfs_frame(tfs_ctx *c, const double *joint, double *axis, double *xdir) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix w, t, m; + + if (c->work(joint, &w, &fflags)) { return -1; } + if (c->tool(joint, &t, &fflags)) { return -1; } + toolFrameInWork(&w, &t, &m); + + axis[0] = m.z.x; axis[1] = m.z.y; axis[2] = m.z.z; + xdir[0] = m.x.x; xdir[1] = m.x.y; xdir[2] = m.x.z; + return 0; +} + +// joint values for a point of the internal search space +static void tfs_joints(tfs_ctx *c, const double *u) +{ + int i; + for (i = 0; i < c->num_joints; i++) { c->joint[i] = c->seed[i]; } + for (i = 0; i < c->nfree; i++) { + c->joint[c->free[i]] = u[i] * c->scale[i]; + } +} + +static int tfs_res(tfs_ctx *c, const double *u, double *r) +{ + double axis[3], xdir[3]; + int i; + + tfs_joints(c, u); + if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } + + for (i = 0; i < 3; i++) { r[i] = axis[i] - c->want[i]; } + if (c->nres > 3) { + for (i = 0; i < 3; i++) { r[3+i] = xdir[i] - c->want[3+i]; } + } + return 0; +} + +static double tfs_norm2(const double *r, int n) +{ + double s = 0; + int i; + for (i = 0; i < n; i++) { s += r[i]*r[i]; } + return s; +} + +static int tfs_jac(tfs_ctx *c, const double *u, double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]) +{ + double up[TOOL_FRAME_MAX_FREE], rp[TFS_MAX_RES], rm[TFS_MAX_RES]; + int i, k; + + for (k = 0; k < c->nfree; k++) { + for (i = 0; i < c->nfree; i++) { up[i] = u[i]; } + up[k] = u[k] + TFS_FD_STEP; + if (tfs_res(c, up, rp)) { return -1; } + up[k] = u[k] - TFS_FD_STEP; + if (tfs_res(c, up, rm)) { return -1; } + for (i = 0; i < c->nres; i++) { + J[i][k] = (rp[i] - rm[i]) / (2*TFS_FD_STEP); + } + } + return 0; +} + +// in place inverse of an n by n matrix by Gauss-Jordan with partial pivoting, +// n being at most TOOL_FRAME_MAX_FREE +static int tfs_inv(double A[TOOL_FRAME_MAX_FREE][TOOL_FRAME_MAX_FREE], int n) +{ + double aug[TOOL_FRAME_MAX_FREE][2*TOOL_FRAME_MAX_FREE]; + int i, j, col, piv; + + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { aug[i][j] = A[i][j]; } + for (j = 0; j < n; j++) { aug[i][n+j] = (i == j) ? 1.0 : 0.0; } + } + for (col = 0; col < n; col++) { + piv = col; + for (i = col+1; i < n; i++) { + if (fabs(aug[i][col]) > fabs(aug[piv][col])) { piv = i; } + } + if (fabs(aug[piv][col]) < 1e-300) { return -1; } + if (piv != col) { + for (j = 0; j < 2*n; j++) { + double sw = aug[col][j]; aug[col][j] = aug[piv][j]; aug[piv][j] = sw; + } + } + { + double d = aug[col][col]; + for (j = 0; j < 2*n; j++) { aug[col][j] /= d; } + } + for (i = 0; i < n; i++) { + double f = aug[i][col]; + if (i == col || f == 0.0) { continue; } + for (j = 0; j < 2*n; j++) { aug[i][j] -= f*aug[col][j]; } + } + } + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { A[i][j] = aug[i][n+j]; } + } + return 0; +} + +// rank by counting pivots, which is all that is needed to say how many +// directions the request leaves free +static int tfs_rank(const double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE], int m, int n) +{ + double a[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]; + double big = 0; + int i, j, col, piv, rank = 0; + + for (i = 0; i < m; i++) { + for (j = 0; j < n; j++) { + a[i][j] = J[i][j]; + if (fabs(a[i][j]) > big) { big = fabs(a[i][j]); } + } + } + if (big <= 0) { return 0; } + + for (col = 0; col < n && rank < m; col++) { + piv = rank; + for (i = rank+1; i < m; i++) { + if (fabs(a[i][col]) > fabs(a[piv][col])) { piv = i; } + } + if (fabs(a[piv][col]) < TFS_RANK_TOL*big) { continue; } + if (piv != rank) { + for (j = 0; j < n; j++) { + double sw = a[rank][j]; a[rank][j] = a[piv][j]; a[piv][j] = sw; + } + } + for (i = rank+1; i < m; i++) { + double f = a[i][col]/a[rank][col]; + for (j = 0; j < n; j++) { a[i][j] -= f*a[rank][j]; } + } + rank++; + } + return rank; +} + +// damped least squares with adaptive damping. Returns 1 when the residual is +// down to the solved threshold, 0 otherwise, and leaves u where it stopped. +static int tfs_levmar(tfs_ctx *c, double *u) +{ + double r[TFS_MAX_RES], r2[TFS_MAX_RES]; + double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]; + double A[TOOL_FRAME_MAX_FREE][TOOL_FRAME_MAX_FREE]; + double g[TOOL_FRAME_MAX_FREE], step[TOOL_FRAME_MAX_FREE]; + double u2[TOOL_FRAME_MAX_FREE]; + double f, f2, lambda = 1e-3; + int i, j, k, it; + + if (tfs_res(c, u, r)) { return 0; } + f = tfs_norm2(r, c->nres); + + for (it = 0; it < TFS_ITERS && f > TFS_SOLVED; it++) { + double trace = 0, big = 0; + + if (tfs_jac(c, u, J)) { return 0; } + + for (i = 0; i < c->nfree; i++) { + for (j = 0; j < c->nfree; j++) { + double s = 0; + for (k = 0; k < c->nres; k++) { s += J[k][i]*J[k][j]; } + A[i][j] = s; + } + trace += A[i][i]; + g[i] = 0; + for (k = 0; k < c->nres; k++) { g[i] += J[k][i]*r[k]; } + } + trace = trace/c->nfree + 1e-30; + + for (i = 0; i < c->nfree; i++) { A[i][i] += lambda*trace; } + if (tfs_inv(A, c->nfree)) { return 0; } + + for (i = 0; i < c->nfree; i++) { + step[i] = 0; + for (j = 0; j < c->nfree; j++) { step[i] -= A[i][j]*g[j]; } + if (fabs(step[i]) > big) { big = fabs(step[i]); } + } + if (big > TFS_STEP_LIMIT) { + for (i = 0; i < c->nfree; i++) { step[i] *= TFS_STEP_LIMIT/big; } + } + for (i = 0; i < c->nfree; i++) { u2[i] = u[i] + step[i]; } + + if (tfs_res(c, u2, r2)) { return 0; } + f2 = tfs_norm2(r2, c->nres); + + if (f2 < f) { + for (i = 0; i < c->nfree; i++) { u[i] = u2[i]; } + for (i = 0; i < c->nres; i++) { r[i] = r2[i]; } + f = f2; + lambda *= 0.3; + if (lambda < 1e-12) { lambda = 1e-12; } + } else { + lambda *= 4.0; + if (lambda > 1e12) { break; } + } + } + return f <= TFS_SOLVED; +} + +static double tfs_wrap(double a) +{ + while (a > PM_PI) { a -= 2*PM_PI; } + while (a < -PM_PI) { a += 2*PM_PI; } + return a; +} + +// which joints turn the tool, and what one turn of each is worth in its own +// units. Returns the count, or -1 if a joint moves the tool without having a +// period, which the search has no way to bound. +static int tfs_survey(tfs_ctx *c) +{ + double base_axis[3], base_x[3], axis[3], xdir[3]; + static const double candidate[2] = { 360.0, 2*PM_PI }; + int i, k, n = 0; + + for (i = 0; i < c->num_joints; i++) { c->joint[i] = c->seed[i]; } + if (tfs_frame(c, c->joint, base_axis, base_x)) { return -1; } + + for (i = 0; i < c->num_joints; i++) { + double moved = 0; + int p; + + for (k = 0; k < c->num_joints; k++) { c->joint[k] = c->seed[k]; } + c->joint[i] = c->seed[i] + 1e-4; + if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } + for (k = 0; k < 3; k++) { + if (fabs(axis[k] - base_axis[k]) > moved) { moved = fabs(axis[k] - base_axis[k]); } + if (fabs(xdir[k] - base_x[k]) > moved) { moved = fabs(xdir[k] - base_x[k]); } + } + if (moved <= TFS_MOVED_TOL) { continue; } + + if (n >= TOOL_FRAME_MAX_FREE) { return -1; } + + c->scale[n] = 0; + for (p = 0; p < 2; p++) { + double back = 0; + c->joint[i] = c->seed[i] + candidate[p]; + if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } + for (k = 0; k < 3; k++) { + if (fabs(axis[k] - base_axis[k]) > back) { back = fabs(axis[k] - base_axis[k]); } + if (fabs(xdir[k] - base_x[k]) > back) { back = fabs(xdir[k] - base_x[k]); } + } + if (back <= TFS_MOVED_TOL) { + c->scale[n] = candidate[p]/(2*PM_PI); + break; + } + } + if (c->scale[n] == 0) { return -1; } + + c->free[n] = i; + n++; + } + c->nfree = n; + return n; +} + +// enumerate the roots for whatever the context currently constrains +static int tfs_search(tfs_ctx *c, + double *solutions, + int max_solutions, + int *free_directions) +{ + double kept[TOOL_FRAME_MAX_SOLUTIONS][TOOL_FRAME_MAX_FREE]; + double u[TOOL_FRAME_MAX_FREE], useed[TOOL_FRAME_MAX_FREE]; + double r[TFS_MAX_RES], J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]; + int index[TOOL_FRAME_MAX_FREE]; + int found = 0, per_axis, first = 1, i, k; + + for (i = 0; i < TOOL_FRAME_MAX_FREE; i++) { u[i] = 0; useed[i] = 0; } + + // nothing on this machine turns the tool, so the only candidate is where + // the machine already is + if (c->nfree == 0) { + if (tfs_res(c, u, r)) { return -1; } + if (tfs_norm2(r, c->nres) > TFS_SOLVED) { return 0; } + for (i = 0; i < c->num_joints; i++) { solutions[i] = c->seed[i]; } + if (free_directions) { free_directions[0] = 0; } + return 1; + } + + for (i = 0; i < c->nfree; i++) { + useed[i] = c->seed[c->free[i]] / c->scale[i]; + index[i] = 0; + } + + // Quarter turns of each free joint, starting from where the machine is so + // that a machine with a free direction reports the answer nearest its + // present pose. Two per turn already enters every basin on the machines + // in the tree, and four is the margin for one that is not: the roots are + // few and widely separated, because they come from the two branches of an + // arc cosine and not from anything finely structured. + per_axis = 4; + + for (;;) { + int solved, rank, dup = 0; + + if (first) { + for (i = 0; i < c->nfree; i++) { u[i] = useed[i]; } + } else { + for (i = 0; i < c->nfree; i++) { + u[i] = -PM_PI + (2*PM_PI*index[i])/per_axis; + } + } + + solved = tfs_levmar(c, u); + if (solved) { + for (i = 0; i < c->nfree; i++) { u[i] = tfs_wrap(u[i]); } + if (tfs_res(c, u, r) || tfs_jac(c, u, J)) { return -1; } + + rank = tfs_rank((const double (*)[TOOL_FRAME_MAX_FREE])J, + c->nres, c->nfree); + tfs_joints(c, u); + + // a rank deficient root means the request does not pin the machine + // down and the answer is a continuum. Report this one point of it + // and say so, rather than returning samples of a curve alongside + // roots that mean something else. + if (c->nfree - rank > 0) { + for (i = 0; i < c->num_joints; i++) { solutions[i] = c->joint[i]; } + if (free_directions) { free_directions[0] = c->nfree - rank; } + return 1; + } + + // Two roots are the same pose if going from one to the other + // does not move the tool. That covers landing on a root already + // found, and it also covers the case a distance test would get + // wrong: near a singularity the search reaches points a long way + // apart in joint values whose frames differ by less than it can + // resolve, and those are one answer and not several. + for (k = 0; k < found; k++) { + double mid[TOOL_FRAME_MAX_FREE] = {0}; + + for (i = 0; i < c->nfree; i++) { + mid[i] = kept[k][i] + tfs_wrap(u[i] - kept[k][i])/2; + } + if (tfs_res(c, mid, r)) { return -1; } + if (tfs_norm2(r, c->nres) <= TFS_SOLVED) { dup = 1; break; } + } + + if (!dup) { + // the dedupe evaluated other points, so rebuild this one + tfs_joints(c, u); + for (i = 0; i < c->num_joints; i++) { + solutions[found*c->num_joints + i] = c->joint[i]; + } + if (free_directions) { free_directions[found] = 0; } + for (i = 0; i < c->nfree; i++) { kept[found][i] = u[i]; } + found++; + if (found >= max_solutions) { return found; } + } + } + + if (first) { first = 0; continue; } + + for (i = 0; i < c->nfree; i++) { + if (++index[i] < per_axis) { break; } + index[i] = 0; + } + if (i == c->nfree) { break; } + } + + return found; +} + +// the turn about the tool axis that carries the tool x this pose achieves onto +// the one the caller asked for +static int tfs_spin(tfs_ctx *c, const double *joint, + const PmCartesian *x_in_work, double *spin) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix w, t, m; + double along_x, along_y; + + if (c->work(joint, &w, &fflags)) { return -1; } + if (c->tool(joint, &t, &fflags)) { return -1; } + toolFrameInWork(&w, &t, &m); + + along_x = m.x.x*x_in_work->x + m.x.y*x_in_work->y + m.x.z*x_in_work->z; + along_y = m.y.x*x_in_work->x + m.y.y*x_in_work->y + m.y.z*x_in_work->z; + + *spin = atan2(along_y, along_x); + return 0; +} + +int toolFrameSolve(kinsFrameFunc work, + kinsFrameFunc tool, + int num_joints, + const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin) +{ + tfs_ctx c; + int found, i; + + if (!work || !tool || !seed || !solutions || !axis_in_work + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS + || max_solutions <= 0) { + return -1; + } + if (max_solutions > TOOL_FRAME_MAX_SOLUTIONS) { + max_solutions = TOOL_FRAME_MAX_SOLUTIONS; + } + + c.work = work; + c.tool = tool; + c.num_joints = num_joints; + c.seed = seed; + c.nres = x_in_work ? 6 : 3; + c.want[0] = axis_in_work->x; + c.want[1] = axis_in_work->y; + c.want[2] = axis_in_work->z; + if (x_in_work) { + double square = axis_in_work->x * x_in_work->x + + axis_in_work->y * x_in_work->y + + axis_in_work->z * x_in_work->z; + + // the two vectors are two axes of one frame, so a request where they + // are not at right angles is not a frame and cannot be reached by + // anything + if (fabs(square) > 1e-6) { return -1; } + + c.want[3] = x_in_work->x; + c.want[4] = x_in_work->y; + c.want[5] = x_in_work->z; + } + + if (tfs_survey(&c) < 0) { return -1; } + + found = tfs_search(&c, solutions, max_solutions, free_directions); + if (found != 0 || !x_in_work) { + if (tool_spin) { + for (i = 0; i < (found > 0 ? found : 0); i++) { tool_spin[i] = 0; } + } + return found; + } + + // The joints cannot place tool x, which is the ordinary case: a five axis + // machine spends both rotaries reaching the tool axis and the turn about + // that axis is not a joint at all. It is still reachable, as a rotation + // of the frame rather than a motion of the machine, so answer with the + // poses that reach the axis and the turn that finishes the job. That is + // what a control does with a Heidenhain base vector or a Fanuc G68.2 + // block, neither of which refuses the program for asking. + if (!tool_spin) { return 0; } + + c.nres = 3; + found = tfs_search(&c, solutions, max_solutions, free_directions); + if (found <= 0) { return found; } + + for (i = 0; i < found; i++) { + if (tfs_spin(&c, solutions + i*num_joints, x_in_work, &tool_spin[i])) { + return -1; + } + } + return found; +} diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 4f62277a287..7e9838c2527 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -44,6 +44,7 @@ static KF kfwds[SWITCHKINS_MAX_TYPES] = {NULL}; static KI kinvs[SWITCHKINS_MAX_TYPES] = {NULL}; static KT ktools[SWITCHKINS_MAX_TYPES] = {NULL}; static KT kworks[SWITCHKINS_MAX_TYPES] = {NULL}; +static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL}; static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; // types provided, counted in rtapi_app_main() once they are all in @@ -248,6 +249,38 @@ int kinematicsWorkFrame(const double *joint, return kworks[switchkins_type](joint, rot, fflags); } // kinematicsWorkFrame() +int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin) +{ + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !ktools[switchkins_type] + || !kworks[switchkins_type]) { + return -1; // this type does not report its frames, so it cannot answer + } + + // a type that derived the answer by hand knows its own degenerate poses + // and is faster than a search, so it wins where it exists + if (ktinvs[switchkins_type]) { + return ktinvs[switchkins_type](axis_in_work, x_in_work, seed, + solutions, max_solutions, + free_directions, tool_spin); + } + + // the dispatch itself is what the search calls, so the native rotation + // and the per-type lookup are already accounted for + return toolFrameSolve(kinematicsWorkFrame, kinematicsToolFrame, + kp.max_joints, + axis_in_work, x_in_work, seed, + solutions, max_solutions, free_directions, + tool_spin); +} // kinematicsToolFrameInverse() + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -302,6 +335,20 @@ int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, return 0; } // switchkinsRegisterFrames() +int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterToolFrameInverse: BAD" + " switchkins_type <%d> (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + ktinvs[ktype] = kinv; + return 0; +} // switchkinsRegisterToolFrameInverse() + //********************************************************************* static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); @@ -315,8 +362,10 @@ EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsToolFrameInverse); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); +EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 7815fe2ab39..e5cffd94853 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -52,4 +52,18 @@ extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); // dispatch. extern int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, const PmRotationMatrix *native); + +// KinematicsTOOLFRAMEINVERSE function (optional, see kinematics.h) +typedef int (*KTI)(const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin); + +// called from switchkinsSetup() only by a type that has a closed form for the +// tool orientation inverse. A type that does not gets the generic search, +// which needs nothing beyond the frames it already registered. +extern int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv); #endif // } diff --git a/tests/tool-frame/test_tool_frame.c b/tests/tool-frame/test_tool_frame.c index dde817ae554..c23cbac8f1e 100644 --- a/tests/tool-frame/test_tool_frame.c +++ b/tests/tool-frame/test_tool_frame.c @@ -12,6 +12,9 @@ #include "emcpos.h" #include "kinematics.h" +#define DEG (M_PI/180.0) +#define NUTATION 45.0 + static int failures; static void check(int ok, const char *what) @@ -50,6 +53,170 @@ static PmRotationMatrix arbitrary(void) -sb, 0, cb); } + +/* ------------------------------------------------------------------ + * Machine models for the tool orientation inverse. + * + * These are the frame functions a module supplies, written out here so the + * solver can be exercised without loading one. Rotary joints are in degrees, + * as every module in the tree takes them, except radMachine, which is in + * radians to prove the solver does not assume. + * ------------------------------------------------------------------ */ + +static PmRotationMatrix rows(const double m[3][3]) +{ + return mat(m[0][0], m[0][1], m[0][2], + m[1][0], m[1][1], m[1][2], + m[2][0], m[2][1], m[2][2]); +} + +static PmRotationMatrix rot_z(double rad) +{ + const double c = cos(rad), s = sin(rad); + const double m[3][3] = {{c, -s, 0}, {s, c, 0}, {0, 0, 1}}; + return rows(m); +} + +/* the nutating secondary joint of the trsrn heads */ +static PmRotationMatrix rot_nutate(double rad) +{ + const double v = NUTATION*DEG, sv = sin(v), cv = cos(v); + const double ss = sin(rad), cs = cos(rad); + const double r = cs + sv*sv*(1 - cs); + const double q = cs + cv*cv*(1 - cs); + const double t = sv*cv*(1 - cs); + const double m[3][3] = {{ cs, -cv*ss, sv*ss}, + {cv*ss, r, t}, + {-sv*ss, t, q}}; + return rows(m); +} + +static PmRotationMatrix product(const PmRotationMatrix *a, + const PmRotationMatrix *b) +{ + const double x[3][3] = {{a->x.x, a->y.x, a->z.x}, + {a->x.y, a->y.y, a->z.y}, + {a->x.z, a->y.z, a->z.z}}; + const double y[3][3] = {{b->x.x, b->y.x, b->z.x}, + {b->x.y, b->y.y, b->z.y}, + {b->x.z, b->y.z, b->z.z}}; + double m[3][3]; + int i, j, k; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + m[i][j] = 0; + for (k = 0; k < 3; k++) { m[i][j] += x[i][k]*y[k][j]; } + } + } + return rows(m); +} + +static int identityFrame(const double *j, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)j; (void)fflags; + *rot = TOOL_FRAME_SPINDLE; + return 0; +} + +/* xyzac: both rotaries carry the table, the tool stays square with the + machine. j[3] is A, j[4] is C. */ +static int xyzacWork(const double *j, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + const double a = j[3]*DEG, c = j[4]*DEG; + const double m[3][3] = {{ cos(c), sin(c), 0}, + {-sin(c)*cos(a), cos(c)*cos(a), sin(a)}, + { sin(c)*sin(a),-cos(c)*sin(a), cos(a)}}; + (void)fflags; + *rot = rows(m); + return 0; +} + +/* a nutating spindle head with nothing turning the work. j[3] is the + nutating secondary joint, j[4] the primary about z. */ +static int headTool(const double *j, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + PmRotationMatrix p = rot_z(j[4]*DEG), s = rot_nutate(j[3]*DEG); + (void)fflags; + *rot = product(&p, &s); + return 0; +} + +/* the same head, in radians, to exercise the period discovery */ +static int radTool(const double *j, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + PmRotationMatrix p = rot_z(j[4]), s = rot_nutate(j[3]); + (void)fflags; + *rot = product(&p, &s); + return 0; +} + +/* a table rotary and a nutating head at once, so three joints turn the tool + and a bare tool axis leaves one of them free. j[3] is the table A, j[4] + the nutating joint, j[5] the head primary. */ +static int mixedWork(const double *j, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + const double a = j[3]*DEG; + const double m[3][3] = {{1, 0, 0}, + {0, cos(a), sin(a)}, + {0, -sin(a), cos(a)}}; + (void)fflags; + *rot = rows(m); + return 0; +} + +static int mixedTool(const double *j, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + PmRotationMatrix p = rot_z(j[5]*DEG), s = rot_nutate(j[4]*DEG); + (void)fflags; + *rot = product(&p, &s); + return 0; +} + +/* what the module would report: the tool frame in workpiece coordinates */ +static PmRotationMatrix in_work(kinsFrameFunc work, kinsFrameFunc tool, + const double *j) +{ + KINEMATICS_FORWARD_FLAGS f = 0; + PmRotationMatrix w, t, out; + + work(j, &w, &f); + tool(j, &t, &f); + toolFrameInWork(&w, &t, &out); + return out; +} + +static int axis_matches(kinsFrameFunc work, kinsFrameFunc tool, + const double *j, const PmCartesian *want) +{ + PmRotationMatrix m = in_work(work, tool, j); + return fabs(m.z.x - want->x) < 1e-9 + && fabs(m.z.y - want->y) < 1e-9 + && fabs(m.z.z - want->z) < 1e-9; +} + +/* does the list hold a solution whose free joints are these, to a degree */ +static int holds(const double *sols, int count, int njoints, + const int *which, const double *value, int n) +{ + int s, i, ok; + + for (s = 0; s < count; s++) { + ok = 1; + for (i = 0; i < n; i++) { + if (fabs(sols[s*njoints + which[i]] - value[i]) > 1e-6) { ok = 0; } + } + if (ok) { return 1; } + } + return 0; +} + int main(void) { PmRotationMatrix m, r; @@ -152,6 +319,250 @@ int main(void) "the work frame is transposed, not applied directly"); } + + /* ------------------------------------------------------------------ + * The tool orientation inverse. + * ------------------------------------------------------------------ */ + { + double seed[6] = {10, 20, 30, 10, 5, 0}; + double truth[6] = {10, 20, 30, 34.4, 68.8, 0}; + double sols[TOOL_FRAME_MAX_SOLUTIONS*6]; + int free_dirs[TOOL_FRAME_MAX_SOLUTIONS]; + double spin[TOOL_FRAME_MAX_SOLUTIONS]; + PmRotationMatrix want; + PmCartesian axis, xdir; + int n, i; + + /* a table rotary machine, tool axis only: the two rotaries pin it + down, and there are two ways to get there */ + want = in_work(xyzacWork, identityFrame, truth); + axis = want.z; + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 2, "xyzac reports both ways to reach a tool axis"); + for (i = 0; i < n; i++) { + check(axis_matches(xyzacWork, identityFrame, sols + i*5, &axis), + "every xyzac solution reaches the requested axis"); + check(free_dirs[i] == 0, "an xyzac solution is pinned down"); + check(sols[i*5 + 0] == seed[0] && sols[i*5 + 1] == seed[1] + && sols[i*5 + 2] == seed[2], + "the joints that do not turn the tool are copied from the seed"); + } + { + const int which[2] = {3, 4}; + const double value[2] = {34.4, 68.8}; + check(holds(sols, n, 5, which, value, 2), + "the pose the request was built from is one of them"); + } + + /* the singular pose: the tool axis is the axis the primary turns + about, so the primary is free and the answer is a family */ + axis.x = 0; axis.y = 0; axis.z = 1; + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 1, "a singular pose reports one representative, not a sample"); + check(free_dirs[0] == 1, "and says one direction is free"); + check(fabs(sols[3]) < 1e-6, "the joint the request does pin down is set"); + check(fabs(sols[4] - seed[4]) < 1e-6, + "the free joint is left where the machine already is"); + + /* approaching the singularity: the two solutions stay two until the + spin about the tool stops being worth anything, and then the answer + becomes the family rather than a scatter of points that differ by + more than the tool can tell apart */ + axis.x = sin(0.01*DEG); axis.y = 0; axis.z = cos(0.01*DEG); + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 2 && free_dirs[0] == 0 && free_dirs[1] == 0, + "a hundredth of a degree off the pole still has two solutions"); + + axis.x = sin(0.001*DEG); axis.y = 0; axis.z = cos(0.001*DEG); + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 1 && free_dirs[0] == 1, + "a thousandth of a degree off it, the spin is free in practice"); + + /* xyzac turns the work through a full sphere, so straight down is a + pose and not a refusal: A at half a turn */ + axis.x = 0; axis.y = 0; axis.z = -1; + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 1 && free_dirs[0] == 1, + "the other pole is reachable, and free about the tool as well"); + check(fabs(fabs(sols[3]) - 180.0) < 1e-6, "reached with A at half a turn"); + + /* a machine where nothing turns the tool answers for the one pose it + has, and refuses anything else */ + axis.x = 0; axis.y = 0; axis.z = 1; + n = toolFrameSolve(identityFrame, identityFrame, 5, &axis, NULL, seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 1 && free_dirs[0] == 0, + "a machine with no orientation joints reports its one pose"); + axis.x = 0; axis.y = 1; axis.z = 0; + n = toolFrameSolve(identityFrame, identityFrame, 5, &axis, NULL, seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 0, "and cannot reach any other"); + + /* out of reach: a nutating head sweeps a cone, and with a nutation + of 45 degrees it cannot get the tool below the horizontal */ + { + double head_seed[5] = {0, 0, 0, 10, 5}; + + axis.x = 0; axis.y = 0; axis.z = -1; + n = toolFrameSolve(identityFrame, headTool, 5, &axis, NULL, + head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + free_dirs, spin); + check(n == 0, "an unreachable axis reports no solutions"); + } + + /* the nutating head, checked against the closed form the TWP remap + uses: cos(secondary) = (Kzz - Cv^2)/(1 - Cv^2), which has the two + roots +theta and -theta */ + { + double head_seed[5] = {0, 0, 0, 10, 5}; + double head_truth[5] = {0, 0, 0, 40.0, 25.0}; + const double cv = cos(NUTATION*DEG); + double closed, s; + + want = in_work(identityFrame, headTool, head_truth); + axis = want.z; + closed = acos((axis.z - cv*cv)/(1 - cv*cv))/DEG; + + n = toolFrameSolve(identityFrame, headTool, 5, &axis, NULL, + head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + free_dirs, spin); + check(n == 2, "the nutating head reports both secondary roots"); + for (i = 0; i < n; i++) { + s = fabs(sols[i*5 + 3]); + check(fabs(s - closed) < 1e-6, + "the search agrees with the closed form of the remap"); + check(axis_matches(identityFrame, headTool, sols + i*5, &axis), + "every nutating solution reaches the requested axis"); + } + check(fabs(sols[0*5 + 3] + sols[1*5 + 3]) < 1e-6, + "the two roots are opposite, as acos gives them"); + + /* the same machine written in radians: the joint unit is + discovered, so the answer is the same shape */ + head_seed[3] = 10*DEG; head_seed[4] = 5*DEG; + n = toolFrameSolve(identityFrame, radTool, 5, &axis, NULL, + head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + free_dirs, spin); + check(n == 2, "a module taking radians is solved too"); + for (i = 0; i < n; i++) { + check(fabs(fabs(sols[i*5 + 3])/DEG - closed) < 1e-6, + "and gives the same angles once the unit is accounted for"); + } + } + + /* three joints turn the tool. A bare tool axis leaves the spin about + it free, and asking for tool x as well pins the machine down. */ + { + double mix_seed[6] = {0, 0, 0, 5, 10, 15}; + double mix_truth[6] = {0, 0, 0, 20, 45.8, 57.3}; + + want = in_work(mixedWork, mixedTool, mix_truth); + axis = want.z; + xdir = want.x; + + n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, NULL, mix_seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 1, "a spare orientation joint gives a family, not a list"); + check(free_dirs[0] == 1, "and one free direction is reported"); + check(axis_matches(mixedWork, mixedTool, sols, &axis), + "the representative reaches the requested axis"); + + for (i = 0; i < TOOL_FRAME_MAX_SOLUTIONS; i++) { spin[i] = 99; } + n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, &xdir, mix_seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 2, "asking for tool x as well pins it down"); + for (i = 0; i < n; i++) { + PmRotationMatrix got = in_work(mixedWork, mixedTool, sols + i*6); + check(free_dirs[i] == 0, "with nothing left free"); + check(spin[i] == 0.0, + "and no turn about the tool left over, the joints did it"); + check(fabs(got.x.x - xdir.x) < 1e-9 + && fabs(got.x.y - xdir.y) < 1e-9 + && fabs(got.x.z - xdir.z) < 1e-9, + "and tool x where it was asked for"); + } + { + const int which[3] = {3, 4, 5}; + const double value[3] = {20, 45.8, 57.3}; + check(holds(sols, n, 6, which, value, 3), + "the pose the request was built from is one of them"); + } + } + + + /* Asking a five axis machine for tool x as well. Its two rotaries are + spent on the tool axis and the turn about that axis is not a joint, + so the answer is the poses that reach the axis plus the turn that + places tool x, which is what the virtual rotation applies. */ + { + double head_seed[5] = {0, 0, 0, 10, 5}; + double head_truth[5] = {0, 0, 0, 40.0, 25.0}; + PmRotationMatrix got, want_frame; + PmCartesian want_x; + double c_s, s_s, dot; + + want_frame = in_work(identityFrame, headTool, head_truth); + axis = want_frame.z; + + /* a tool x at right angles to that axis, but not the one this + machine happens to produce: turn the achieved one by 30 degrees + about the axis */ + c_s = cos(30*DEG); s_s = sin(30*DEG); + want_x.x = c_s*want_frame.x.x + s_s*want_frame.y.x; + want_x.y = c_s*want_frame.x.y + s_s*want_frame.y.y; + want_x.z = c_s*want_frame.x.z + s_s*want_frame.y.z; + + n = toolFrameSolve(identityFrame, headTool, 5, &axis, &want_x, + head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + free_dirs, spin); + check(n == 2, "the axis is still reached both ways"); + for (i = 0; i < n; i++) { + check(axis_matches(identityFrame, headTool, sols + i*5, &axis), + "every solution reaches the requested axis"); + got = in_work(identityFrame, headTool, sols + i*5); + /* turning the achieved frame by the reported spin has to land + tool x where it was asked for */ + c_s = cos(spin[i]); s_s = sin(spin[i]); + check(fabs(c_s*got.x.x + s_s*got.y.x - want_x.x) < 1e-9 + && fabs(c_s*got.x.y + s_s*got.y.y - want_x.y) < 1e-9 + && fabs(c_s*got.x.z + s_s*got.y.z - want_x.z) < 1e-9, + "and the reported turn places tool x"); + } + + /* with nowhere to report the turn, the request cannot be answered + rather than being answered wrongly */ + n = toolFrameSolve(identityFrame, headTool, 5, &axis, &want_x, + head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + free_dirs, NULL); + check(n == 0, "and without somewhere to put it, no solutions"); + + /* the two vectors are two axes of one frame */ + dot = 0.5; + want_x.x = axis.x + dot; want_x.y = axis.y; want_x.z = axis.z; + check(toolFrameSolve(identityFrame, headTool, 5, &axis, &want_x, + head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + free_dirs, spin) == -1, + "a tool x not at right angles to the axis is refused"); + } + + /* the arguments are checked rather than trusted */ + axis.x = 0; axis.y = 0; axis.z = 1; + check(toolFrameSolve(NULL, identityFrame, 5, &axis, NULL, seed, sols, + TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1, + "a missing frame function is refused"); + check(toolFrameSolve(xyzacWork, identityFrame, 0, &axis, NULL, seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1, + "a bogus joint count is refused"); + check(toolFrameSolve(xyzacWork, identityFrame, 5, NULL, NULL, seed, + sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1, + "a missing target is refused"); + } + if (failures) { printf("%d failure(s)\n", failures); return 1; } printf("all tool frame checks passed\n"); return 0; From 2bf57f8d76c5fa861bac5d7863a0d59c85abd5f9 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 26 Aug 2026 21:49:16 +1000 Subject: [PATCH 18/58] trsrn: set the kinematics pins up once, at load Both modules build their pins on the first kinematicsType() call and test an is_setup flag that nothing ever sets, so every later call runs the setup again. The second run reassigns haldata to a fresh block and then fails to create the pins that block points at, all of them already taken, so the module is left reading through null references and the next forward or inverse call takes realtime with it. Motion asks twice when num_extrajoints is greater than zero, once in rtapi_app_main and once in init_comm_buffers, so that combination cannot be running today. Latching the flag would stop the repeat, but the setup does not belong in a function whose job is to answer a question. halcompile has a hook for this, used by homecomp for the same reason: EXTRA_SETUP() runs once from the generated setup, before the component is made ready. So the pins exist from the moment the module is loaded, kinematicsType() only answers, and the hal_set_unready() and hal_ready() calls the old placement needed are gone with it. --- src/hal/components/xyzacb_trsrn.comp | 11 +++++------ src/hal/components/xyzbca_trsrn.comp | 9 +++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 098af8a7aa2..4efedcae6ce 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -7,6 +7,7 @@ FIXME """; pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; +option extra_setup; license "GPL"; author "David Mueller"; @@ -43,13 +44,14 @@ static struct haldata { } *haldata; -static int xyzacb_trsrn_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; #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; haldata = hal_malloc(sizeof(struct haldata)); if (!haldata) goto error; @@ -76,7 +78,6 @@ static int xyzacb_trsrn_setup(void) { 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: @@ -136,8 +137,6 @@ int kinematicsSwitch(int new_switchkins_type) KINEMATICS_TYPE kinematicsType() { -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 diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 12e40344125..763b2801c33 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -7,6 +7,7 @@ FIXME """; pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; +option extra_setup; license "GPL"; author "David Mueller"; @@ -43,7 +44,10 @@ static struct haldata { } *haldata; -static int xyzbca_trsrn_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; #define HAL_PREFIX "xyzbca_trsrn_kins" int res=0; // inbherit comp_id from rtapi_main() @@ -76,7 +80,6 @@ static int xyzbca_trsrn_setup(void) { 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: @@ -136,8 +139,6 @@ int kinematicsSwitch(int new_switchkins_type) KINEMATICS_TYPE kinematicsType() { -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 From 1cbc8ceac0908ae84c94c8cce66ee674c338214e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 26 Aug 2026 19:07:39 +1000 Subject: [PATCH 19/58] tests: check the shipped frame functions where they run The frame cases so far run machines written for the test, so the hand-written matrix entries in each module are compiled and nothing more, and that is where a sign or a transposed pair hides. Run them in service instead: a realtime component loaded after the module under test, reaching it through the same exported entry points motion uses, for every kinematics type the module offers. A failed check fails the load, and a failed load fails the test. The work frame is checked against the forward kinematics rather than against a matrix copied out of the module: a row of the work frame is how the reported position responds to one machine axis, by central difference. The tool frame has no such tie on a machine that carries the work, so what is checked is that the frame is a rotation, that a spindle the module calls fixed never moves, and that a joint carrying the whole head turns the reported frame about the machine's z and nothing else. The last one catches a frame built for the wrong joint or composed in the wrong order. Checked by mutation, in failed checks: reversing the head composition order in a trsrn module 300, a flipped sign in its work frame 150, one in the xyzac work frame 18, the wrong spindle convention there 25, a flipped sign in the puma flange 72. The tree as it stands fails none. --- tests/kins-frames/checkresult | 3 + tests/kins-frames/framecheck.c | 293 +++++++++++++++++++++++++++++++++ tests/kins-frames/skip | 4 + tests/kins-frames/test.sh | 53 ++++++ 4 files changed, 353 insertions(+) create mode 100755 tests/kins-frames/checkresult create mode 100644 tests/kins-frames/framecheck.c create mode 100755 tests/kins-frames/skip create mode 100755 tests/kins-frames/test.sh diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult new file mode 100755 index 00000000000..5fefd687bac --- /dev/null +++ b/tests/kins-frames/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +[ "$(grep -c 'frames agree' "$1")" = 5 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c new file mode 100644 index 00000000000..a2be114cc68 --- /dev/null +++ b/tests/kins-frames/framecheck.c @@ -0,0 +1,293 @@ +/* Check a kinematics module's reported frames where they run in service. + * + * Loaded after the module under test, so kinematicsForward(), + * kinematicsWorkFrame() and kinematicsToolFrame() resolve to it. A + * failed check fails the load, and a failed load fails the test. + * + * The work frame has a tie to the forward kinematics and is checked + * against it: a row of it is how the reported position responds to one + * machine axis, measured here by central difference. + * + * The tool frame has no such tie on a machine that carries the work. + * Its forward reports the rotary joint values, which describe how the + * work is turned, and say nothing about where the tool points. So the + * tool frame is checked for what can be checked: that it is a rotation, + * that a spindle the module calls fixed never moves, and that a joint + * which turns the whole head about the machine's z turns the reported + * frame with it and does nothing else. That last one catches a frame + * built for the wrong joint or composed in the wrong order. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("kinematics frame checker"); + +static int joints = 5; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); + +static int carries_tool = 0; +RTAPI_MP_INT(carries_tool, "1 when the machine carries the tool rather than the work"); + +static int fixed_spindle = 0; +RTAPI_MP_INT(fixed_spindle, "1 when the module reports a spindle square with the machine"); + +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type where the module models its own machine"); + +static int spin = -1; +RTAPI_MP_INT(spin, "joint that turns the whole head about the machine's z, -1 for none"); + +static int r1 = -1, r2 = -1, r3 = -1; +RTAPI_MP_INT(r1, "joint number of the first rotary to sweep"); +RTAPI_MP_INT(r2, "joint number of the second rotary, -1 for none"); +RTAPI_MP_INT(r3, "joint number of the third rotary, -1 for none"); + +/* switchkins.h is not an exported header, and a module rejects a type + it does not have, so the loop only needs an upper bound */ +#define MAX_TYPES 9 + +#define TO_RAD (M_PI / 180.0) +#define STEP 1e-6 +#define TURN 15.0 +#define TOL 1e-6 + +static int comp_id = -1; +static int failures; + +static void expect(int ok, const char *what, const double *j) +{ + char pose[128]; + int i, n = 0; + + if (ok) { return; } + for (i = 0; i < joints && n < (int)sizeof(pose) - 12; i++) { + n += rtapi_snprintf(pose + n, sizeof(pose) - n, "%s%.4g", + i ? "," : "", j[i]); + } + rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: FAIL %s at [%s]\n", what, pose); + failures++; +} + +static int close3(const PmCartesian *a, double x, double y, double z) +{ + return fabs(a->x - x) < TOL && fabs(a->y - y) < TOL && fabs(a->z - z) < TOL; +} + +/* The helpers in kins_util.c are not exported to a loadable module, and + working the answers out here is the better test anyway: nothing the + module under test uses is reused to judge it. */ +static double dot(const PmCartesian *a, const PmCartesian *b) +{ + return a->x * b->x + a->y * b->y + a->z * b->z; +} + +static int is_rotation(const PmRotationMatrix *m) +{ + PmCartesian cross; + + if (fabs(dot(&m->x, &m->x) - 1) > TOL) { return 0; } + if (fabs(dot(&m->y, &m->y) - 1) > TOL) { return 0; } + if (fabs(dot(&m->z, &m->z) - 1) > TOL) { return 0; } + if (fabs(dot(&m->x, &m->y)) > TOL) { return 0; } + if (fabs(dot(&m->x, &m->z)) > TOL) { return 0; } + if (fabs(dot(&m->y, &m->z)) > TOL) { return 0; } + + /* right handed, so the third column is the cross product of the + other two rather than its negative */ + cross.x = m->x.y * m->y.z - m->x.z * m->y.y; + cross.y = m->x.z * m->y.x - m->x.x * m->y.z; + cross.z = m->x.x * m->y.y - m->x.y * m->y.x; + return close3(&cross, m->z.x, m->z.y, m->z.z); +} + +/* how the reported position responds to a displacement of machine axis + jno: column jno of the forward transform's linear part */ +static void response(const double *j, int jno, PmCartesian *out) +{ + double t[EMCMOT_MAX_JOINTS]; + EmcPose lo, hi; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + + memcpy(t, j, sizeof(t)); + + t[jno] = j[jno] - STEP; + kinematicsForward(t, &lo, &ff, &inf); + t[jno] = j[jno] + STEP; + kinematicsForward(t, &hi, &ff, &inf); + + out->x = (hi.tran.x - lo.tran.x) / (2 * STEP); + out->y = (hi.tran.y - lo.tran.y) / (2 * STEP); + out->z = (hi.tran.z - lo.tran.z) / (2 * STEP); +} + +/* turn a frame about the machine's z, which is what a joint carrying + the whole head does to everything above it */ +static void turn_about_z(double deg, const PmRotationMatrix *m, + PmRotationMatrix *out) +{ + const double c = cos(deg * TO_RAD); + const double s = sin(deg * TO_RAD); + + out->x.x = c * m->x.x - s * m->x.y; + out->x.y = s * m->x.x + c * m->x.y; + out->x.z = m->x.z; + out->y.x = c * m->y.x - s * m->y.y; + out->y.y = s * m->y.x + c * m->y.y; + out->y.z = m->y.z; + out->z.x = c * m->z.x - s * m->z.y; + out->z.y = s * m->z.x + c * m->z.y; + out->z.z = m->z.z; +} + +/* Reporting the frames is optional, and a switchable module usually + supplies them for some of its types and not others, so a type that + declines is skipped rather than failed. */ +static int supplies_frames(const double *j) +{ + KINEMATICS_FORWARD_FLAGS ff = 0; + PmRotationMatrix m; + + if (kinematicsWorkFrame(j, &m, &ff)) { return 0; } + if (kinematicsToolFrame(j, &m, &ff)) { return 0; } + return 1; +} + +static void check(const double *j, int own_kinematics) +{ + KINEMATICS_FORWARD_FLAGS ff = 0; + PmRotationMatrix work, tool, turned, want; + PmCartesian d; + double t[EMCMOT_MAX_JOINTS]; + + kinematicsWorkFrame(j, &work, &ff); + kinematicsToolFrame(j, &tool, &ff); + + expect(is_rotation(&work), "work frame is a rotation", j); + expect(is_rotation(&tool), "tool frame is a rotation", j); + + if (carries_tool) { + /* nothing turns the work, at any pose */ + expect(close3(&work.x, 1, 0, 0) && close3(&work.y, 0, 1, 0) + && close3(&work.z, 0, 0, 1), "work frame is the machine frame", j); + } else { + /* the forward transform maps a machine displacement to a work + one, so a row of the work frame is one of its columns */ + response(j, 0, &d); + expect(close3(&d, work.x.x, work.y.x, work.z.x), "work frame against X", j); + response(j, 1, &d); + expect(close3(&d, work.x.y, work.y.y, work.z.y), "work frame against Y", j); + response(j, 2, &d); + expect(close3(&d, work.x.z, work.y.z, work.z.z), "work frame against Z", j); + } + + /* the rest describes the module's own machine, so its other + kinematics types, identity and the tool frame's own, are not + asked: they leave everything square with the machine */ + if (!own_kinematics) { return; } + + if (fixed_spindle) { + expect(close3(&tool.x, 1, 0, 0) && close3(&tool.y, 0, 1, 0) + && close3(&tool.z, 0, 0, 1), "the spindle stays square", j); + } + + if (spin >= 0) { + memcpy(t, j, sizeof(t)); + t[spin] = j[spin] + TURN; + kinematicsToolFrame(t, &turned, &ff); + turn_about_z(TURN, &tool, &want); + expect(close3(&turned.x, want.x.x, want.x.y, want.x.z), + "tool x turns with the head", j); + expect(close3(&turned.y, want.y.x, want.y.y, want.y.z), + "tool y turns with the head", j); + expect(close3(&turned.z, want.z.x, want.z.y, want.z.z), + "tool axis turns with the head", j); + } +} + +int rtapi_app_main(void) +{ + /* rotary values away from the identity, including the quarter and + half turns where a sine changes sign or a cosine vanishes */ + static const double angle[] = { 0, 30, -25, 90, 180 }; + const int angles = sizeof(angle) / sizeof(angle[0]); + double j[EMCMOT_MAX_JOINTS]; + int a, b, c, t; + int checked = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { + rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: joints=%d\n", joints); + return -1; + } + + comp_id = hal_init("framecheck"); + if (comp_id < 0) { return comp_id; } + + if (kinematicsType() == 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: the module reports no type\n"); + hal_exit(comp_id); + return -1; + } + + memset(j, 0, sizeof(j)); + if (!carries_tool) { j[0] = 10; j[1] = 20; j[2] = 30; } + + /* every kinematics the module offers, not just the one it starts + in: the frames a switchable module reports are per type, and the + type that turns the work is rarely the default */ + for (t = 0; t < MAX_TYPES; t++) { + if (kinematicsSwitchable() && kinematicsSwitch(t)) { break; } + if (!supplies_frames(j)) { continue; } + checked++; + + for (a = 0; a < angles; a++) { + if (r1 >= 0) { j[r1] = angle[a]; } + for (b = 0; b < angles; b++) { + if (r2 >= 0) { j[r2] = angle[b]; } + for (c = 0; c < angles; c++) { + if (r3 >= 0) { j[r3] = angle[c]; } + check(j, t == ktype); + if (r3 < 0) { break; } + } + if (r2 < 0) { break; } + } + if (r1 < 0) { break; } + } + + if (!kinematicsSwitchable()) { break; } + } + + if (!checked) { + rtapi_print_msg(RTAPI_MSG_ERR, + "framecheck: the module reports frames for no type\n"); + hal_exit(comp_id); + return -1; + } + + if (failures) { + rtapi_print_msg(RTAPI_MSG_ERR, + "framecheck: %d check(s) failed\n", failures); + hal_exit(comp_id); + return -1; + } + + rtapi_print("framecheck: frames agree for %d kinematics type(s)\n", checked); + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-frames/skip b/tests/kins-frames/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-frames/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-frames/test.sh b/tests/kins-frames/test.sh new file mode 100755 index 00000000000..4bb8e108765 --- /dev/null +++ b/tests/kins-frames/test.sh @@ -0,0 +1,53 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install framecheck.c >/dev/null + +# One hal file per module: they all define the same entry points, so +# only one can be loaded at a time. +run() { + local hal + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s\n' "$1" + printf '%s\n' "$2" + printf 'loadrt framecheck %s\n' "$3" + } > "$hal" + echo "=== $1" + halrun -f "$hal" + rm -f "$hal" +} + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4 fixed_spindle=1" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.y-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4 fixed_spindle=1" + +# a nutation angle of zero leaves the head square with the machine and +# the interesting geometry untested, so give both a real one +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.tool-offset-z 50" \ + "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1" + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.tool-offset-z 50" \ + "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1" + +run "pumakins" "setp pumakins.A2 300" \ + "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0" From 7ae6452ba11443e49da526d0c6316cc6a7ed7bad Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 19:11:47 +1000 Subject: [PATCH 20/58] kinematics: the tool frame is the joints' alone The header said that a module applying a virtual rotation about the tool axis reports a frame that includes it. No module does, and none should: the rotation is a rotation of the coordinate system, not of the machine, which is where Heidenhain's COORD ROT, Fanuc's feature coordinate system and Siemens' swivel frame keep it. Reporting the joints alone also keeps the orientation inverse honest, since the turn it reports about the tool axis is then the value to apply and not a difference from whatever is applied already. Found by putting the two trsrn components next to the tilted work plane python: the frames agree exactly at zero virtual rotation and differ by exactly Rz(pre-rot) otherwise. Say so in the header and the chapter. --- docs/src/motion/kinematics-conventions.adoc | 10 ++++++++++ src/emc/kinematics/kinematics.h | 10 ++++++++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 6d30f7ed630..667e835bd80 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -207,6 +207,16 @@ names. A machine with no such pin has no say in the matter: its tool X is whatever the chain produces, and a consumer that needs a defined one applies the rotation itself. +The frame a module reports is the joints' alone and does not carry the virtual +rotation. The rotation is a rotation of the coordinate system, not of the +machine, and that is where the other controls keep it: Heidenhain's +`COORD ROT` rotates the working plane, Fanuc's `G68.2` feature coordinate +system carries its own X, Siemens' swivel puts the residual turn in the frame. +Reporting the joints alone also keeps the orientation inverse honest: the turn +it reports about the tool axis is then the value to apply, not a difference +from whatever is applied already. A consumer that wants tool X as programmed, +a preview or a model, multiplies the frame by the rotation on the pin. + [IMPORTANT] By default, tool X lies parallel to the machine XY plane. Where the tool axis is vertical and that leaves tool X free, tool X is machine X. `G68.3 R` rotates diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index e6c459c3179..9f82ac15185 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -121,8 +121,14 @@ extern KINEMATICS_TYPE kinematicsType(void); confused with the tool length, which is the distance applied along it. It runs from the tool tip towards the holder. The origin of the tool frame is the controlled point that kinematicsForward() reports for the same joints. - Where a module applies a virtual rotation about the tool axis, the frame - returned includes it. + + The frame is what the joints do. The virtual rotation about the tool axis + that a tilted work plane applies, the pre-rot pin on the in-tree + components, is not part of it: it is a rotation of the coordinate system, + applied by whoever programs in the frame, which is where Heidenhain's + COORD ROT, Fanuc's feature coordinate system and Siemens' swivel frame keep + it as well. A consumer that wants tool x as programmed multiplies the + frame by that rotation itself; it has the pin. A module whose own maths is in the other sense, which is every module built on the ISO 9787 flange frame or on Denavit-Hartenberg parameters, does not From 275210f0bac9c5509a770c06efafc7f2bf962e79 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 19:14:39 +1000 Subject: [PATCH 21/58] kinematics: let the caller hold joints in the tool frame inverse A rotary table turns the tool against the work as surely as a head rotary does, so on a machine with a table and a two axis head the solver had three orientation joints and a bare tool axis left a family, of which it reported the member nearest the seed; asked for tool x as well it turned the table to place it and reported no spin. The tilted work plane remap does the opposite: it holds the table, orients the head and applies the spin as the virtual rotation. Neither is wrong, and which is wanted is a machining decision; Heidenhain names the axes a tilt may use with M138 and lets PLANE choose TABLE ROT or COORD ROT. So the request names the joints to hold, a bit per joint, zero for none. A held joint keeps its seed value and the request is solved with the rest. Holding the table gives the two head solutions and the spin that finishes the frame, which is what the remap computes. --- src/emc/kinematics/kinematics.h | 13 +++++ src/emc/kinematics/kins_util.c | 6 +++ src/emc/kinematics/switchkins.c | 5 +- src/emc/kinematics/switchkins.h | 1 + tests/tool-frame/test_tool_frame.c | 77 +++++++++++++++++++++++------- 5 files changed, 82 insertions(+), 20 deletions(-) diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 9f82ac15185..8eea2dcdfd7 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -272,6 +272,17 @@ extern int toolFrameIsProper(const PmRotationMatrix *m); breaks the tie where a machine has more orientation joints than the request constrains. + held is a bit per joint, bit n for joint n, naming the joints the caller + does not want moved; they keep their seed value and the request is solved + with the rest. Zero lets every joint that turns the tool take part. This + is the caller's policy and not the module's: a table rotary turns the tool + against the work as surely as a head rotary does, so with nothing held a + machine with a table and a two axis head has a spare orientation joint, and + a bare tool axis leaves a family. A tilted work plane that keeps the table + where it is, as the TWP remap does and as Heidenhain's M138 says, holds it + and gets the two head solutions and the spin about the tool that finishes + the frame. + solutions receives max_solutions complete sets of joint values, one after another, each num_joints long. free_directions, if not NULL, receives one entry per solution: 0 where the joints are pinned down, and n where the @@ -295,6 +306,7 @@ extern int toolFrameIsProper(const PmRotationMatrix *m); extern int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, const PmCartesian *x_in_work, const double *seed, + unsigned int held, double *solutions, int max_solutions, int *free_directions, @@ -316,6 +328,7 @@ extern int toolFrameSolve(kinsFrameFunc work, const PmCartesian *axis_in_work, const PmCartesian *x_in_work, const double *seed, + unsigned int held, double *solutions, int max_solutions, int *free_directions, diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index ee9d6593c1d..1e271c72659 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -546,6 +546,7 @@ typedef struct { kinsFrameFunc tool; int num_joints; const double *seed; + unsigned int held; // bit per joint the caller keeps still int nfree; int free[TOOL_FRAME_MAX_FREE]; double scale[TOOL_FRAME_MAX_FREE]; // joint units per internal radian @@ -779,6 +780,9 @@ static int tfs_survey(tfs_ctx *c) double moved = 0; int p; + // a held joint stays at its seed value whatever it could do + if (c->held & (1u << i)) { continue; } + for (k = 0; k < c->num_joints; k++) { c->joint[k] = c->seed[k]; } c->joint[i] = c->seed[i] + 1e-4; if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } @@ -947,6 +951,7 @@ int toolFrameSolve(kinsFrameFunc work, const PmCartesian *axis_in_work, const PmCartesian *x_in_work, const double *seed, + unsigned int held, double *solutions, int max_solutions, int *free_directions, @@ -968,6 +973,7 @@ int toolFrameSolve(kinsFrameFunc work, c.tool = tool; c.num_joints = num_joints; c.seed = seed; + c.held = held; c.nres = x_in_work ? 6 : 3; c.want[0] = axis_in_work->x; c.want[1] = axis_in_work->y; diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 7e9838c2527..ac2a83d8006 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -252,6 +252,7 @@ int kinematicsWorkFrame(const double *joint, int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, const PmCartesian *x_in_work, const double *seed, + unsigned int held, double *solutions, int max_solutions, int *free_directions, @@ -267,7 +268,7 @@ int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, // a type that derived the answer by hand knows its own degenerate poses // and is faster than a search, so it wins where it exists if (ktinvs[switchkins_type]) { - return ktinvs[switchkins_type](axis_in_work, x_in_work, seed, + return ktinvs[switchkins_type](axis_in_work, x_in_work, seed, held, solutions, max_solutions, free_directions, tool_spin); } @@ -276,7 +277,7 @@ int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, // and the per-type lookup are already accounted for return toolFrameSolve(kinematicsWorkFrame, kinematicsToolFrame, kp.max_joints, - axis_in_work, x_in_work, seed, + axis_in_work, x_in_work, seed, held, solutions, max_solutions, free_directions, tool_spin); } // kinematicsToolFrameInverse() diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index e5cffd94853..e9023f6288f 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -57,6 +57,7 @@ extern int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, typedef int (*KTI)(const PmCartesian *axis_in_work, const PmCartesian *x_in_work, const double *seed, + unsigned int held, double *solutions, int max_solutions, int *free_directions, diff --git a/tests/tool-frame/test_tool_frame.c b/tests/tool-frame/test_tool_frame.c index c23cbac8f1e..db17d3589eb 100644 --- a/tests/tool-frame/test_tool_frame.c +++ b/tests/tool-frame/test_tool_frame.c @@ -337,7 +337,7 @@ int main(void) down, and there are two ways to get there */ want = in_work(xyzacWork, identityFrame, truth); axis = want.z; - n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 2, "xyzac reports both ways to reach a tool axis"); for (i = 0; i < n; i++) { @@ -358,7 +358,7 @@ int main(void) /* the singular pose: the tool axis is the axis the primary turns about, so the primary is free and the answer is a family */ axis.x = 0; axis.y = 0; axis.z = 1; - n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 1, "a singular pose reports one representative, not a sample"); check(free_dirs[0] == 1, "and says one direction is free"); @@ -371,13 +371,13 @@ int main(void) becomes the family rather than a scatter of points that differ by more than the tool can tell apart */ axis.x = sin(0.01*DEG); axis.y = 0; axis.z = cos(0.01*DEG); - n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 2 && free_dirs[0] == 0 && free_dirs[1] == 0, "a hundredth of a degree off the pole still has two solutions"); axis.x = sin(0.001*DEG); axis.y = 0; axis.z = cos(0.001*DEG); - n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 1 && free_dirs[0] == 1, "a thousandth of a degree off it, the spin is free in practice"); @@ -385,7 +385,7 @@ int main(void) /* xyzac turns the work through a full sphere, so straight down is a pose and not a refusal: A at half a turn */ axis.x = 0; axis.y = 0; axis.z = -1; - n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, + n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 1 && free_dirs[0] == 1, "the other pole is reachable, and free about the tool as well"); @@ -394,12 +394,12 @@ int main(void) /* a machine where nothing turns the tool answers for the one pose it has, and refuses anything else */ axis.x = 0; axis.y = 0; axis.z = 1; - n = toolFrameSolve(identityFrame, identityFrame, 5, &axis, NULL, seed, + n = toolFrameSolve(identityFrame, identityFrame, 5, &axis, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 1 && free_dirs[0] == 0, "a machine with no orientation joints reports its one pose"); axis.x = 0; axis.y = 1; axis.z = 0; - n = toolFrameSolve(identityFrame, identityFrame, 5, &axis, NULL, seed, + n = toolFrameSolve(identityFrame, identityFrame, 5, &axis, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 0, "and cannot reach any other"); @@ -410,7 +410,7 @@ int main(void) axis.x = 0; axis.y = 0; axis.z = -1; n = toolFrameSolve(identityFrame, headTool, 5, &axis, NULL, - head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + head_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 0, "an unreachable axis reports no solutions"); } @@ -429,7 +429,7 @@ int main(void) closed = acos((axis.z - cv*cv)/(1 - cv*cv))/DEG; n = toolFrameSolve(identityFrame, headTool, 5, &axis, NULL, - head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + head_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 2, "the nutating head reports both secondary roots"); for (i = 0; i < n; i++) { @@ -446,7 +446,7 @@ int main(void) discovered, so the answer is the same shape */ head_seed[3] = 10*DEG; head_seed[4] = 5*DEG; n = toolFrameSolve(identityFrame, radTool, 5, &axis, NULL, - head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + head_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 2, "a module taking radians is solved too"); for (i = 0; i < n; i++) { @@ -465,7 +465,7 @@ int main(void) axis = want.z; xdir = want.x; - n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, NULL, mix_seed, + n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, NULL, mix_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 1, "a spare orientation joint gives a family, not a list"); check(free_dirs[0] == 1, "and one free direction is reported"); @@ -473,7 +473,7 @@ int main(void) "the representative reaches the requested axis"); for (i = 0; i < TOOL_FRAME_MAX_SOLUTIONS; i++) { spin[i] = 99; } - n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, &xdir, mix_seed, + n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, &xdir, mix_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 2, "asking for tool x as well pins it down"); for (i = 0; i < n; i++) { @@ -492,6 +492,47 @@ int main(void) check(holds(sols, n, 6, which, value, 3), "the pose the request was built from is one of them"); } + + /* the same machine with the table held, which is what a tilted + work plane that leaves the table alone asks: the head alone + reaches the axis two ways, and tool x is then a turn about the + tool rather than a table move */ + n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, NULL, mix_seed, + 1u << 3, sols, TOOL_FRAME_MAX_SOLUTIONS, + free_dirs, spin); + check(n == 2, "holding the table leaves the two head solutions"); + for (i = 0; i < n; i++) { + check(free_dirs[i] == 0, "with nothing left free"); + check(sols[i*6 + 3] == mix_seed[3], "and the table where it was"); + check(axis_matches(mixedWork, mixedTool, sols + i*6, &axis), + "every held-table solution reaches the requested axis"); + } + + n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, &xdir, mix_seed, + 1u << 3, sols, TOOL_FRAME_MAX_SOLUTIONS, + free_dirs, spin); + check(n == 2, "tool x with the table held is still reached both ways"); + for (i = 0; i < n; i++) { + PmRotationMatrix got = in_work(mixedWork, mixedTool, sols + i*6); + double c_s = cos(spin[i]), s_s = sin(spin[i]); + check(sols[i*6 + 3] == mix_seed[3], "the table is still where it was"); + check(spin[i] != 0.0, "so the turn about the tool is not zero"); + check(fabs(c_s*got.x.x + s_s*got.y.x - xdir.x) < 1e-9 + && fabs(c_s*got.x.y + s_s*got.y.y - xdir.y) < 1e-9 + && fabs(c_s*got.x.z + s_s*got.y.z - xdir.z) < 1e-9, + "and it places tool x"); + } + + /* holding every orientation joint leaves the one pose */ + n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, NULL, mix_truth, + (1u << 3) | (1u << 4) | (1u << 5), sols, + TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 1 && free_dirs[0] == 0, + "with everything held, the seed answers if it reaches the axis"); + n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, NULL, mix_seed, + (1u << 3) | (1u << 4) | (1u << 5), sols, + TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 0, "and refuses if it does not"); } @@ -518,7 +559,7 @@ int main(void) want_x.z = c_s*want_frame.x.z + s_s*want_frame.y.z; n = toolFrameSolve(identityFrame, headTool, 5, &axis, &want_x, - head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + head_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); check(n == 2, "the axis is still reached both ways"); for (i = 0; i < n; i++) { @@ -537,7 +578,7 @@ int main(void) /* with nowhere to report the turn, the request cannot be answered rather than being answered wrongly */ n = toolFrameSolve(identityFrame, headTool, 5, &axis, &want_x, - head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + head_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, NULL); check(n == 0, "and without somewhere to put it, no solutions"); @@ -545,20 +586,20 @@ int main(void) dot = 0.5; want_x.x = axis.x + dot; want_x.y = axis.y; want_x.z = axis.z; check(toolFrameSolve(identityFrame, headTool, 5, &axis, &want_x, - head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS, + head_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1, "a tool x not at right angles to the axis is refused"); } /* the arguments are checked rather than trusted */ axis.x = 0; axis.y = 0; axis.z = 1; - check(toolFrameSolve(NULL, identityFrame, 5, &axis, NULL, seed, sols, + check(toolFrameSolve(NULL, identityFrame, 5, &axis, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1, "a missing frame function is refused"); - check(toolFrameSolve(xyzacWork, identityFrame, 0, &axis, NULL, seed, + check(toolFrameSolve(xyzacWork, identityFrame, 0, &axis, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1, "a bogus joint count is refused"); - check(toolFrameSolve(xyzacWork, identityFrame, 5, NULL, NULL, seed, + check(toolFrameSolve(xyzacWork, identityFrame, 5, NULL, NULL, seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1, "a missing target is refused"); } From 23862930bb1333a2b6c1440e8af3a6e6ba7ea17d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 19:14:49 +1000 Subject: [PATCH 22/58] kinematics: take the tool frame inverse request as a program writes it The search solves to a residual of 1e-18, and a direction written to six digits is a unit vector only to 1e-7, so a request copied from a program was reported unreachable. Normalise the axis on the way in, and take the component along it off the requested tool x, refusing only a zero vector or a tool x within a millionth of a radian of the axis, since neither describes a frame. --- docs/src/motion/kinematics-conventions.adoc | 18 ++++++++ src/emc/kinematics/kinematics.h | 6 +++ src/emc/kinematics/kins_util.c | 41 ++++++++++++------ tests/tool-frame/test_tool_frame.c | 46 +++++++++++++++++++++ 4 files changed, 99 insertions(+), 12 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 667e835bd80..3708cc68081 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -339,6 +339,24 @@ along with the number of directions left free. A caller that hands back a list of samples from a curve as though they were alternatives is telling the operator something false. +Which joints may take part is the caller's decision, not the module's. A +rotary table turns the tool against the work as surely as a head rotary does, +so a machine with a table and a two-axis head has three orientation joints and +a bare tool axis leaves one of them free. Whether the table is used for that +is a machining choice: Heidenhain names the axes a tilt may use with `M138`, +and its `PLANE` lets the operator pick `TABLE ROT`, turn the table to place +tool X, or `COORD ROT`, leave the table and rotate the coordinate system. So +the request names the joints to hold. Holding the table gives the two head +solutions and the turn about the tool that finishes the frame, which is what +the TWP remap computes; holding nothing lets the table place tool X and the +turn comes back zero. + +The request is taken as a program carries it. A direction written to a few +digits is a unit vector only to within its rounding, and a tool X written the +same way is at right angles to the axis only to within it, so both are +normalised on the way in and only a vector that is not a direction at all is +refused. + === What a module has to supply Nothing, if it already reports its frames. The shared code answers the question diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 8eea2dcdfd7..83edfcad791 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -283,6 +283,12 @@ extern int toolFrameIsProper(const PmRotationMatrix *m); and gets the two head solutions and the spin about the tool that finishes the frame. + The request is normalised on the way in: axis_in_work is scaled to unit + length and x_in_work has its component along the axis removed, so the + rounded numbers a program carries do not make an orientation unreachable. + A zero vector, or a tool x within a millionth of a radian of lying along + the axis, is still refused, since neither describes a frame. + solutions receives max_solutions complete sets of joint values, one after another, each num_joints long. free_directions, if not NULL, receives one entry per solution: 0 where the joints are pinned down, and n where the diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 1e271c72659..fa20010d1d2 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -958,6 +958,7 @@ int toolFrameSolve(kinsFrameFunc work, double *tool_spin) { tfs_ctx c; + double axis[3], xdir[3], len; int found, i; if (!work || !tool || !seed || !solutions || !axis_in_work @@ -969,28 +970,44 @@ int toolFrameSolve(kinsFrameFunc work, max_solutions = TOOL_FRAME_MAX_SOLUTIONS; } + // The request as a program carries it, a few digits of each component, + // is a unit vector only to within its rounding, and the search solves + // to well below that. Normalise on the way in, and refuse only what is + // not a direction at all. + axis[0] = axis_in_work->x; + axis[1] = axis_in_work->y; + axis[2] = axis_in_work->z; + len = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); + if (len < 1e-12) { return -1; } + for (i = 0; i < 3; i++) { axis[i] /= len; } + c.work = work; c.tool = tool; c.num_joints = num_joints; c.seed = seed; c.held = held; c.nres = x_in_work ? 6 : 3; - c.want[0] = axis_in_work->x; - c.want[1] = axis_in_work->y; - c.want[2] = axis_in_work->z; + for (i = 0; i < 3; i++) { c.want[i] = axis[i]; } if (x_in_work) { - double square = axis_in_work->x * x_in_work->x - + axis_in_work->y * x_in_work->y - + axis_in_work->z * x_in_work->z; + double along; + + xdir[0] = x_in_work->x; + xdir[1] = x_in_work->y; + xdir[2] = x_in_work->z; + len = sqrt(xdir[0]*xdir[0] + xdir[1]*xdir[1] + xdir[2]*xdir[2]); + if (len < 1e-12) { return -1; } + for (i = 0; i < 3; i++) { xdir[i] /= len; } // the two vectors are two axes of one frame, so a request where they // are not at right angles is not a frame and cannot be reached by - // anything - if (fabs(square) > 1e-6) { return -1; } - - c.want[3] = x_in_work->x; - c.want[4] = x_in_work->y; - c.want[5] = x_in_work->z; + // anything; within rounding of right angles, the component along + // the axis is rounding and comes off + along = axis[0]*xdir[0] + axis[1]*xdir[1] + axis[2]*xdir[2]; + if (fabs(along) > 1e-6) { return -1; } + for (i = 0; i < 3; i++) { xdir[i] -= along*axis[i]; } + len = sqrt(xdir[0]*xdir[0] + xdir[1]*xdir[1] + xdir[2]*xdir[2]); + if (len < 1e-12) { return -1; } + for (i = 0; i < 3; i++) { c.want[3+i] = xdir[i]/len; } } if (tfs_survey(&c) < 0) { return -1; } diff --git a/tests/tool-frame/test_tool_frame.c b/tests/tool-frame/test_tool_frame.c index db17d3589eb..00103cb4678 100644 --- a/tests/tool-frame/test_tool_frame.c +++ b/tests/tool-frame/test_tool_frame.c @@ -535,6 +535,52 @@ int main(void) check(n == 0, "and refuses if it does not"); } + /* the request as a program writes it: a direction to six digits is + not a unit vector, and must not be unreachable for that */ + { + PmCartesian rounded, unit; + double len; + + want = in_work(xyzacWork, identityFrame, truth); + rounded.x = floor(want.z.x*1e6 + 0.5)/1e6; + rounded.y = floor(want.z.y*1e6 + 0.5)/1e6; + rounded.z = floor(want.z.z*1e6 + 0.5)/1e6; + len = sqrt(rounded.x*rounded.x + rounded.y*rounded.y + rounded.z*rounded.z); + unit.x = rounded.x/len; unit.y = rounded.y/len; unit.z = rounded.z/len; + check(fabs(len - 1.0) > 1e-9, "the rounded request is off unit length"); + + n = toolFrameSolve(xyzacWork, identityFrame, 5, &rounded, NULL, seed, + 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + check(n == 2, "a rounded axis is solved"); + for (i = 0; i < n; i++) { + check(axis_matches(xyzacWork, identityFrame, sols + i*5, &unit), + "to the direction it names"); + } + + /* a tool x off right angles by rounding is taken, a zero axis is not */ + { + double head_seed[5] = {0, 0, 0, 10, 5}; + PmRotationMatrix f = in_work(identityFrame, headTool, truth); + PmCartesian x_off; + const double c_s = cos(30*DEG), s_s = sin(30*DEG); + /* an x the head cannot make, turned 30 degrees about the + axis, then nudged off right angles by rounding */ + x_off.x = c_s*f.x.x + s_s*f.y.x + 1e-8*f.z.x; + x_off.y = c_s*f.x.y + s_s*f.y.y + 1e-8*f.z.y; + x_off.z = c_s*f.x.z + s_s*f.y.z + 1e-8*f.z.z; + axis = f.z; + n = toolFrameSolve(identityFrame, headTool, 5, &axis, &x_off, + head_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS, + free_dirs, spin); + check(n == 2, "a tool x off right angles by rounding is taken"); + check(n == 2 && spin[0] != 0.0, "and answered with a turn about the tool"); + } + rounded.x = 0; rounded.y = 0; rounded.z = 0; + check(toolFrameSolve(xyzacWork, identityFrame, 5, &rounded, NULL, seed, + 0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1, + "a zero vector is refused"); + } + /* Asking a five axis machine for tool x as well. Its two rotaries are spent on the tool axis and the turn about that axis is not a joint, From 797fdf78920b14f7e456ed935d542accf7e8b77f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:23 +1000 Subject: [PATCH 23/58] xyzbc-trt-kins: apply the direction sign to the offsets in the inverse The inverse turns the x and z offsets through the B tilt in the conventional sense, while the forward turns them in whichever sense the conventional-directions pin selects. With the pin false, which is the default, and any of x-offset, z-offset or tool-offset set, a pose does not survive the round trip: the position comes back out by twice sin(b) times the offset. xyzac has no term of this kind. Found by tests/kins-jacobian, which multiplies the derivative of the inverse by differences of the forward and expects the identity. --- src/emc/kinematics/trtfuncs.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 31c9ff1a6de..0cb4b5eb7aa 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -362,11 +362,14 @@ int xyzbcKinematicsInverse(const EmcPose * pos, const double dz = hal_get_real(haldata->z_offset) + dt; const double b_rad = pos->b*TO_RAD; const double c_rad = pos->c*TO_RAD; - const double dpx = -cos(b_rad)*dx + sin(b_rad)*dz + dx; - const double dpz = -sin(b_rad)*dx - cos(b_rad)*dz + dz; rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + // the offsets seen from the tilted table: the same rotation the + // forward applies to them, in the same sense + const double dpx = -cos(b_rad)*dx + con * sin(b_rad)*dz + dx; + const double dpz = -con * sin(b_rad)*dx - cos(b_rad)*dz + dz; + EmcPose P; // computed position P.tran.x = + cos(c_rad) * cos(b_rad) * (pos->tran.x - x_rot_point) From f913560918f2be147fe7a80ca27e59646f6a872b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:23 +1000 Subject: [PATCH 24/58] scarakins: set the elbow flag for a negative elbow angle The forward sets the flag that makes the inverse negate its arc cosine when joint 1 is below 90 degrees, so for an elbow between 0 and 90 the inverse returns the other arm and the pose does not survive the round trip. The sign of the arc cosine is the sign of the elbow angle, so the test is against zero. --- src/emc/kinematics/scarakins.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 2454a67bfb2..0ab0bd921de 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -96,8 +96,9 @@ int scaraKinematicsForward(const double * joint, z = D1 + D3 - joint[2] - D5; c = a3; + // the elbow flag: which sign the inverse gives the acos of joint 1 *iflags = 0; - if (joint[1] < 90) + if (joint[1] < 0) *iflags = 1; world->tran.x = x; From c0fc16d2b63f0d83caa693754a3104194384ab2f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:23 +1000 Subject: [PATCH 25/58] pumakins, three21kins: compare the branch flags modulo a whole turn The forward decides the shoulder, elbow and wrist branches by comparing a joint angle with the value the inverse's formula gives, within a fuzz, and does not wrap the difference. A joint standing a whole turn from that value, which the differences of two atan2 results produce freely, fails the comparison and the inverse is sent down the other branch. The difference is brought into (-pi, pi] first. --- src/emc/kinematics/pumakins.c | 20 +++++++++++++++----- src/emc/kinematics/three21kins.c | 20 +++++++++++++++----- 2 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 5aaa4066193..22374507be2 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -29,6 +29,16 @@ struct haldata { hal_real_t a2, a3, d3, d4, d6; } *haldata = NULL; +/* the difference of two angles, brought into (-pi, pi] so that a joint a + whole turn from the formula still matches it */ +static double angleDiff(double a, double b) +{ + double d = a - b; + while (d > PM_PI) { d -= 2*PM_PI; } + while (d <= -PM_PI) { d += 2*PM_PI; } + return d; +} + /* The flange orientation for a joint set: the ISO 9787 mechanical interface frame, whose z points out of the interface towards the work. Shared by the forward kinematics and the tool frame so the two cannot drift apart. */ @@ -152,16 +162,16 @@ static int pumaKinematicsForward(const double * joint, *iflags = 0; /* Set shoulder-up flag if necessary */ - if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) + - atan2(PUMA_D3, -sqrt(sumSq))) < FLAG_FUZZ) + if (fabs(angleDiff(joint[0]*PM_PI/180, atan2(hom.tran.y, hom.tran.x) - + atan2(PUMA_D3, -sqrt(sumSq)))) < FLAG_FUZZ) { *iflags |= PUMA_SHOULDER_RIGHT; } /* Set elbow down flag if necessary */ - if (fabs(joint[2]*PM_PI/180 - atan2(PUMA_A3, PUMA_D4) + + if (fabs(angleDiff(joint[2]*PM_PI/180, atan2(PUMA_A3, PUMA_D4) - atan2(k, -sqrt(PUMA_A3 * PUMA_A3 + - PUMA_D4 * PUMA_D4 - k * k))) < FLAG_FUZZ) + PUMA_D4 * PUMA_D4 - k * k)))) < FLAG_FUZZ) { *iflags |= PUMA_ELBOW_DOWN; } @@ -177,7 +187,7 @@ static int pumaKinematicsForward(const double * joint, /* if not singular set wrist flip flag if necessary */ else{ - if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ)) + if (! (fabs(angleDiff(joint[3]*PM_PI/180, atan2(t1, t2))) < FLAG_FUZZ)) { *iflags |= PUMA_WRIST_FLIP; } diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 219f3877427..ebe45c46412 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -32,6 +32,16 @@ struct haldata { hal_real_t a1, a2, a3, d1, d2, d3, d4, d6; } *haldata = NULL; +/* the difference of two angles, brought into (-pi, pi] so that a joint a + whole turn from the formula still matches it */ +static double angleDiff(double a, double b) +{ + double d = a - b; + while (d > PM_PI) { d -= 2*PM_PI; } + while (d <= -PM_PI) { d += 2*PM_PI; } + return d; +} + static int three21KinematicsForward(const double * joint, EmcPose * world, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -132,8 +142,8 @@ static int three21KinematicsForward(const double * joint, *iflags = 0; /* set shoulder flag */ - if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) + - atan2(d23, -sqrt(sumSq))) < FLAG_FUZZ) + if (fabs(angleDiff(joint[0]*PM_PI/180, atan2(hom.tran.y, hom.tran.x) - + atan2(d23, -sqrt(sumSq)))) < FLAG_FUZZ) { *iflags |= THREE21_SHOULDER_RIGHT; } @@ -143,8 +153,8 @@ static int three21KinematicsForward(const double * joint, if (discr < 0.0) { discr = 0.0; } - if (fabs(joint[2]*PM_PI/180 - atan2(a3, d4) + - atan2(k, -sqrt(discr))) < FLAG_FUZZ) + if (fabs(angleDiff(joint[2]*PM_PI/180, atan2(a3, d4) - + atan2(k, -sqrt(discr)))) < FLAG_FUZZ) { *iflags |= THREE21_ELBOW_DOWN; } @@ -158,7 +168,7 @@ static int three21KinematicsForward(const double * joint, } else { - if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ)) + if (! (fabs(angleDiff(joint[3]*PM_PI/180, atan2(t1, t2))) < FLAG_FUZZ)) { *iflags |= THREE21_WRIST_FLIP; } From b6f1856a157aa1087ff0624bba42f7cedd6fd0a9 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 26/58] userkins, millturn, matrixkins, xyzab_tdr_kins: set the kinematics pins up once, at load The four remaining modules build their pins on the first kinematicsType() call and test an is_setup flag that nothing ever sets, so every later call runs the setup again, reassigns haldata to a fresh block and fails to create the pins that block points at, all of them already taken. Motion asks twice when num_extrajoints is greater than zero, and any second caller sees the same. The setup moves to EXTRA_SETUP(), which halcompile runs once from the generated setup before the component is made ready, as the two trsrn modules already do. The pins then exist from load, kinematicsType() only answers, and the hal_set_unready() and hal_ready() calls the old placement needed are gone with it. userkins is the template for out of tree modules, so its description changes to say where the pins go. --- src/hal/components/matrixkins.comp | 14 +++++--------- src/hal/components/millturn.comp | 13 ++++++------- src/hal/components/userkins.comp | 18 +++++++++--------- src/hal/components/xyzab_tdr_kins.comp | 11 +++++------ 4 files changed, 25 insertions(+), 31 deletions(-) diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index b12dedc2fcf..aac6c04d913 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -176,6 +176,7 @@ the adjustment values should be added to the old values instead of replacing the """; see_also "kins(9)"; pin out bool dummy=1; // halcompile requires at least one pin +option extra_setup; license "GPL"; ;; @@ -191,15 +192,15 @@ static struct haldata { hal_real_t C_zz; } *haldata; -static int matrixkins_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; int res=0; // inherit comp_id from rtapi_main() if (comp_id < 0) goto error; - res = hal_set_unready(comp_id); - if (res) goto error; - haldata = hal_malloc(sizeof(struct haldata)); if (!haldata) goto error; @@ -215,9 +216,6 @@ static int matrixkins_setup(void) { if (res) goto error; - res = hal_ready(comp_id); - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); return 0; error: @@ -235,8 +233,6 @@ EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() { - static bool is_setup=0; - if (!is_setup) matrixkins_setup(); return KINEMATICS_BOTH; } diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index dbf18cd3a1c..62d5980ed29 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -27,9 +27,10 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// Use the *_setup() function for pins and params used by kinematics. +// Use EXTRA_SETUP() 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"; @@ -59,14 +60,15 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -static int millturn_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; #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; @@ -85,7 +87,6 @@ static int millturn_setup(void) { 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: @@ -132,8 +133,6 @@ int kinematicsSwitch(int new_switchkins_type) 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 diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index a2a25d88c29..a7af5d29a75 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -53,14 +53,16 @@ change all instances of `userkins` to `mykins`. * The *fpin* pin is included to satisfy the requirements of the halcompile utility but it is not accessible to kinematics functions. * HAL pins and parameters needed in kinematics functions (kinematicsForward(), - kinematicsInverse()) must be setup in a function (*userkins_setup()*) invoked - by the initial motion module call to kinematicsType(). + kinematicsInverse()) must be setup in the *EXTRA_SETUP()* function, which + halcompile runs once when the module is loaded, before the component is + made ready. """; // The fpin pin is not accessible in kinematics functions. -// Use the *_setup() function for pins and params used by kinematics. +// Use EXTRA_SETUP() 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 "Dewey Garrett"; @@ -91,14 +93,15 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -static int userkins_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; #define HAL_PREFIX "userkins" 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(struct haldata)); if (!haldata) goto error; @@ -112,7 +115,6 @@ static int userkins_setup(void) { res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); if (res) goto error; - hal_ready(comp_id); rtapi_print("*** %s setup ok\n",__FILE__); return 0; error: @@ -130,8 +132,6 @@ EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() { -static bool is_setup=0; - if (!is_setup) userkins_setup(); return KINEMATICS_IDENTITY; // set as required // Note: If kinematics are identity, using KINEMATICS_BOTH // may be used in order to allow a gui to display diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index ea9e39839a0..2d7a71b966d 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -31,6 +31,7 @@ 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"; @@ -54,13 +55,14 @@ static struct haldata { hal_bool_t kinstype_is_1; } *haldata; -static int xyzab_tdr_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; #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; @@ -80,7 +82,6 @@ static int xyzab_tdr_setup(void) { 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: @@ -127,8 +128,6 @@ int kinematicsSwitch(int new_switchkins_type) 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 From 9417334781d72204199d6a9946186e95608a74b8 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 27/58] kinematics: add an optional Jacobian entry point A feed is a speed in the work frame and what the machine has to deliver is a speed at each joint; on any kinematics that is not the identity the two are related by where the machine is. Nothing in the interface answers that relation, so a limit taken from the joints rather than from static per-axis INI values has nowhere to get it. kinematicsJacobian() answers it: jac[j][a] is how joint j responds to a unit rate of pose coordinate a, rows joints, columns in EmcPose order. It is the derivative of kinematicsInverse(), because that is what every consumer multiplies by, because every module has an inverse where the forward may iterate or be absent, and because a gantry with two joints on one letter is two rows of 1 where the other direction has no unique inverse. Entries are in joint units per pose unit, whatever the module's own forward and inverse use, so nothing is converted and the interface does not have to name the rotary joint unit. The answer lives where the pose lives, the work frame, and the rotary columns are rates of the pose words, not an angular velocity; the frames stay against the machine, and the chapter says why the two rules differ. A module supplies nothing: kinsJacobianFromInverse() takes central differences of its inverse about the pose, eighteen inverse calls on the branch the inverse flags select. Modules built on switchkins.c answer for every type, exactly for an identity type and by differences for a type that registers nothing; switchkinsRegisterJacobian() takes a closed form where a module has one. kinsJacobianFromMappedAxes() turns the derivative of a computed position into rows for the modules that finish in position_to_mapped_joints(), duplicates included. Nothing in motion calls it yet; that is the realtime seam of the limits work, and it waits on the closed forms because differencing costs eighteen inverse calls a pose. --- docs/src/motion/kinematics-conventions.adoc | 99 ++++++++++++++- src/emc/kinematics/kinematics.h | 95 +++++++++++++++ src/emc/kinematics/kins_util.c | 126 ++++++++++++++++++++ src/emc/kinematics/switchkins.c | 40 +++++++ src/emc/kinematics/switchkins.h | 11 ++ src/emc/kinematics/trivkins.c | 9 ++ 6 files changed, 374 insertions(+), 6 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 3708cc68081..84ea1b09a5d 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -279,12 +279,11 @@ The joint values that reach a requested orientation, through question a tilted work plane asks when it has to orient the machine. <> says what it answers. -The Jacobian, relating commanded velocity to joint velocity at a given pose, so -that a feed can be checked against the joint velocity, acceleration and limit -values it will actually demand, and so that proximity to a singularity is a -number rather than a surprise. A module with a closed form can supply it -directly. Otherwise it can be obtained by differencing `kinematicsInverse()` -about the pose, which needs no change to the module at all. +How joint motion follows world motion, through `kinematicsJacobian()`, so +that a feed can be checked against the joint velocity and acceleration it will +actually demand, and so that proximity to a singularity is a number rather +than a surprise. <> says what it answers and in +which units. All of these are functions of the joint values and the module's own geometry. None needs state carried between calls, and none needs the module to be running @@ -379,6 +378,88 @@ The search is not a realtime routine. How long it takes depends on the machine and on the request, and the callers that want it, orienting a tilted work plane and previewing a program, are not in the servo loop. +[[sec:jacobian]] +== The Jacobian + +A feed is a speed in the work frame. What the machine has to deliver is a +speed at each joint, and on any kinematics that is not the identity the two +are related by where the machine is. The Jacobian is that relation at one +pose: how each joint responds to a unit rate of each pose coordinate. + + jac[j][a] = d joint[j] / d pose[a] + +Rows are joints. Columns are the pose coordinates in `EmcPose` order, X Y Z A +B C U V W. It is the derivative of `kinematicsInverse()`: multiplied by a pose +velocity it gives the joint velocity motion will command, which is what a feed +limit compares with the joint limits. Joint `j` binds when + + |jac[j] . tangent| * F + +exceeds that joint's velocity limit, `tangent` being the direction of the move +in pose coordinates and `F` the feed along it. The acceleration limit follows +from a second Jacobian taken further along the path, with no more from the +module. A row that grows without bound is a pose approaching a singularity, +where no world speed is slow enough for the joints to follow. + +=== Units + +Each entry is in joint units per pose unit, whatever units the module's own +forward and inverse already use. Nothing is converted: a caller that feeds +pose rates in `EmcPose` units gets joint rates in the units motion already +commands, and never has to know which unit a rotary joint is in. On every +module in the tree both are degrees, so a table rotary's own row is a 1 in its +own column, and a robot's rotary rows carry degrees per millimetre against the +linear columns. + +This is why the Jacobian, unlike the orientation inverse, does not need the +interface to name the rotary joint unit. Every number in it is a ratio of +quantities that already pass through `kinematicsForward()` and +`kinematicsInverse()`, and the caller never combines it with anything measured +in another unit. + +=== Frame + +The columns are pose coordinates, so the answer lives in the work frame, where +`kinematicsForward()` reports positions. The A, B and C columns are rates of +the pose words, the wrapped linear axes the planner already treats as +coordinates, and not an angular velocity vector: on a machine that carries the +work the forward writes the rotary joint into the pose word, and that column +says exactly that, a 1 for its own joint. + +That makes this a different object from the frames of +<>, and the two rules are kept apart deliberately. A frame +is an orientation, and a renderer placing two bodies needs each against +something fixed, so frames are reported against the machine. A Jacobian is a +derivative of the pose, and everything that uses it multiplies it by a pose +rate, so it is reported where the pose is. A module whose maths produces a +twist in the machine frame, which is what the Denavit-Hartenberg modules +produce, turns it into pose word rates through the matrix of the axes each +pose word turns about, once, inside the module. `genserkins` does this, and +having it written once there is worth more than the closed form itself, since +every consumer would otherwise guess it. + +=== What a module has to supply + +Nothing. The shared code takes central differences of the module's inverse +about the pose, eighteen inverse calls on the solution branch the inverse +flags select. That costs a few microseconds on a closed form inverse and +milliseconds on one that iterates, and it answers to the inverse's own +precision, which for an iterating inverse is its convergence tolerance divided +by the step. Modules built on `switchkins.c` answer this way for every type +that registers nothing; an identity type answers exactly. + +A module with a closed form registers it with `switchkinsRegisterJacobian()`. +It is exact, it costs what the inverse costs, and it knows its own singular +poses rather than discovering them as an inverse that fails a step away from +the pose. Every module in the tree whose inverse is written out supplies one. +The two arms whose inverse is a chain of arc tangents, `pumakins` and +`three21kins`, answer through the differences. + +A module reading its rotary angles from the joint argument of the inverse +rather than from the pose, which the nutating heads do, has an inverse whose +derivative about the pose is not the coupling the machine has. Such a module +supplies the closed form, taken against the pose. + [[sec:writing-a-module]] == Writing a Module @@ -408,6 +489,12 @@ Orientation inverse:: do nothing. Register a closed form only where one exists, and where it does, say which poses it treats as degenerate. +Jacobian:: + Rows are joints, columns are pose coordinates, entries in the units the + forward and inverse already use, reported where the pose is. A module with + a closed form inverse differentiates it and registers the result; one + without lets the shared code difference the inverse. + Geometry stays in the module:: Whatever a consumer needs to know about the machine's shape is answered by the module. A consumer that restates it has taken a copy that nothing keeps diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 83edfcad791..2a6c6a0b750 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -16,6 +16,7 @@ #define __LINUXCNC_KINEMATICS_H #include "emcpos.h" /* EmcPose */ +#include "emcmotcfg.h" /* EMCMOT_MAX_JOINTS, EMCMOT_MAX_AXIS */ #include "rtapi_bool.h" /* @@ -340,6 +341,90 @@ extern int toolFrameSolve(kinsFrameFunc work, int *free_directions, double *tool_spin); +/* How each joint responds to a unit rate of each pose coordinate: + + jac[j][a] = d joint[j] / d pose[a] + + Rows are joints, columns are pose coordinates in EmcPose order, x y z a b + c u v w. This is the derivative of kinematicsInverse(): multiply it by a + pose velocity and the result is the joint velocity that motion will + command, which is what a feed limit checks against the joint limits. A + row that grows without bound is a pose approaching a singularity, where + the joints cannot keep up with any world speed at all. + + Each entry is in joint units per pose unit, whatever units the module's + own forward and inverse already use. Nothing is converted here: a caller + that feeds pose rates in EmcPose units gets joint rates in the units + motion already commands, and never has to know which unit a rotary joint + is in. On every module in the tree both are degrees, so a table rotary's + own row is a plain 1 in its own column. + + The columns are pose coordinates, so the answer lives in the work frame, + where kinematicsForward() reports positions. The a, b and c columns are + rates of the pose words, the wrapped linear axes the planner already + treats as coordinates, and not an angular velocity vector. That makes + this a different object from the frames above, which are orientations + and are given against the machine; see the Kinematics Conventions + chapter. + + joint and world are one pose in both descriptions: world is what + kinematicsForward() reports for joint under these flags. Both are given + because a closed form differentiates at the joints while the generic + default perturbs the pose, and iflags keeps every inverse the default + calls on the same solution branch. Rows past the module's joint count + are zero. + + Optional, like the frames. Modules built on switchkins.c export it + always and answer for every type, since it can always be obtained from + the inverse where a frame cannot; other modules need not export it, and + a caller that resolves it dynamically and finds nothing can call + kinsJacobianFromInverse() itself with the module's inverse. + + Returns 0, or -1 if the module cannot answer at this pose. */ +extern int kinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kinsInverseFunc)(const EmcPose *world, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +/* The generic Jacobian, by central differences of an inverse about world: + two inverse calls per pose coordinate, eighteen in all, on the solution + branch iflags selects. The joint array handed to every call starts from + joint, so a module that reads its joint argument sees the machine where + it is. + + The answer is as good as the inverse: a closed form gives it to rounding, + an inverse that iterates to a tolerance gives it to that tolerance over + the step, and should supply its own. num_joints is the module's joint + count. Returns 0, or -1 if any inverse fails. */ +#define KINS_JACOBIAN_STEP 1e-3 /* pose units, either kind */ + +extern int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* For a module whose inverse computes a position P and then hands it to + position_to_mapped_joints(): given dP[axis][pose], how each coordinate of + P responds to each pose coordinate, fill in jac so that every joint gets + the row of the letter it is mapped to. Duplicate letters get duplicate + rows, which is the gantry case. */ +extern int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* joints are axes: a 1 per joint in the column of its letter */ +extern int identityKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); //NOTE: switchable kinematics may require Interp::Synch @@ -392,6 +477,11 @@ extern int xyzacKinematicsWorkFrame(const double *joints, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +extern int xyzacKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int xyzbcKinematicsForward(const double *joints, EmcPose * pos, @@ -411,4 +501,9 @@ extern int xyzbcKinematicsWorkFrame(const double *joints, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +extern int xyzbcKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + //********************************************************************* diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index fa20010d1d2..2e30969fd90 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1040,3 +1040,129 @@ int toolFrameSolve(kinsFrameFunc work, } return found; } + +//---------------------------------------------------------------------- +// The Jacobian. See kinematics.h for what it is and which way it points. +//---------------------------------------------------------------------- + +static void kj_zero(double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int j, a; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } +} + +// pose coordinate a of p, in EmcPose order +static double *kj_coord(EmcPose *p, int a) +{ + switch (a) { + 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; + } +} + +int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose p; + int j, a; + + if (!inverse || !joint || !world || !jac + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + + kj_zero(jac); + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p = *world; + // the joint array every call sees starts at the machine's own + // position, for a module that reads it before writing it + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&p, a) += KINS_JACOBIAN_STEP; + if (inverse(&p, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&p, a) -= 2 * KINS_JACOBIAN_STEP; + if (inverse(&p, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < num_joints; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsJacobianFromInverse() + +int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int jno, a; + + if (!map_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsJacobianFromMappedAxes before map_initialized\n"); + return -1; + } + if (max_joints <= 0 || max_joints > EMCMOT_MAX_JOINTS) { return -1; } + + kj_zero(jac); + + for (jno = 0; jno < max_joints; jno++) { + int bit = 1<= kins_count) { + return -1; + } + // a closed form is exact and knows its own singular poses + if (kjacs[switchkins_type]) { + return kjacs[switchkins_type](joint, world, jac, iflags); + } + // otherwise the type's own inverse, differenced. The type function + // rather than the dispatch, so this cannot recurse through a switch. + if (!kinvs[switchkins_type]) { return -1; } + return kinsJacobianFromInverse(kinvs[switchkins_type], kp.max_joints, + joint, world, iflags, jac); +} // kinematicsJacobian() + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -336,6 +356,20 @@ int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, return 0; } // switchkinsRegisterFrames() +int switchkinsRegisterJacobian(int ktype, KJ kjac) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterJacobian: BAD switchkins_type" + " <%d> (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + kjacs[ktype] = kjac; + return 0; +} // switchkinsRegisterJacobian() + int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -364,9 +398,11 @@ EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); +EXPORT_SYMBOL(switchkinsRegisterJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -403,6 +439,10 @@ int rtapi_app_main(void) ktools[i] = identityKinematicsToolFrame; knative[i] = TOOL_FRAME_SPINDLE; } + // and its Jacobian is exact, so do not difference for it + if (!kjacs[i] && kfwds[i] == identityKinematicsForward) { + kjacs[i] = identityKinematicsJacobian; + } } // the highest type provided by either route sets the count diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index e9023f6288f..c76355b0e00 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -67,4 +67,15 @@ typedef int (*KTI)(const PmCartesian *axis_in_work, // tool orientation inverse. A type that does not gets the generic search, // which needs nothing beyond the frames it already registered. extern int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv); + +// KinematicsJACOBIAN function (optional, see kinematics.h) +typedef int (*KJ)(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +// called from switchkinsSetup() only by a type with a closed form. A type +// that does not gets the exact answer if it is an identity type, and +// otherwise the generic differences of its own inverse. +extern int switchkinsRegisterJacobian(int ktype, KJ kjac); #endif // } diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 4b3685dc6d6..f04d9642622 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -52,6 +52,14 @@ int kinematicsWorkFrame(const double *joints, return identityKinematicsWorkFrame(joints, rot, fflags); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + return identityKinematicsJacobian(joints, pos, jac, iflags); +} + static KINEMATICS_TYPE ktype = -1; KINEMATICS_TYPE kinematicsType() @@ -72,6 +80,7 @@ EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; From 7d429b2e2e3ad96b1682d3ae6eda04d0972bfcce Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 28/58] trtfuncs, 5axiskins, maxkins: supply the Jacobian Each inverse is a rotation of the pose about the table or the pivot plus offsets, so the derivative is the same rotation for the linear columns and the rotation advanced a quarter turn, times the lever, for the rotary ones. The tables and 5axiskins build the position and hand it to position_to_mapped_joints(), so they fill a matrix of the position's derivative and let kinsJacobianFromMappedAxes() place the rows, which keeps duplicate letters right. maxkins has fixed joint numbers and fills its rows directly. --- src/emc/kinematics/5axiskins.c | 44 ++++++++++++ src/emc/kinematics/maxkins.c | 45 ++++++++++++ src/emc/kinematics/trtfuncs.c | 105 ++++++++++++++++++++++++++++ src/emc/kinematics/xyzac-trt-kins.c | 2 + src/emc/kinematics/xyzbc-trt-kins.c | 2 + 5 files changed, 198 insertions(+) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 387c32e23df..027d4a8205a 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -159,6 +159,48 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, return 0; } // fiveaxis_kinematicsInverse() +static int fiveaxis_KinematicsJacobian(const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)joints; + (void)iflags; + rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double R = pivot_length + pos->w; + const double sb = sin(TO_RAD*pos->b), cb = cos(TO_RAD*pos->b); + const double sc = sin(TO_RAD*pos->c), cc = cos(TO_RAD*pos->c); + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } + } + + // the computed position of the inverse is the pose less the pivot + // vector r = s2r(R, c, 180 - b), which is (R sin b cos c, R sin b sin c, + // -R cos b); each row is that coordinate differentiated + dP[0][0] = 1; + dP[0][4] = -R * cb * cc * TO_RAD; + dP[0][5] = R * sb * sc * TO_RAD; + dP[0][8] = -sb * cc; + + dP[1][1] = 1; + dP[1][4] = -R * cb * sc * TO_RAD; + dP[1][5] = -R * sb * cc * TO_RAD; + dP[1][8] = -sb * sc; + + dP[2][2] = 1; + dP[2][4] = -R * sb * TO_RAD; + dP[2][8] = cb; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxes(fiveaxis_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // fiveaxis_KinematicsJacobian() + int fiveaxis_KinematicsSetup(const int comp_id, const char* coordinates, kparms* kp) @@ -255,11 +297,13 @@ int switchkinsSetup(kparms* kp, *kset1 = fiveaxis_KinematicsSetup; *kfwd1 = fiveaxis_KinematicsForward; *kinv1 = fiveaxis_KinematicsInverse; + switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = fiveaxis_KinematicsSetup; *kfwd0 = fiveaxis_KinematicsForward; *kinv0 = fiveaxis_KinematicsInverse; + switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index edd242206d7..28cf4edebeb 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -112,6 +112,50 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double k = M_PI/180; + const double sb = sin(d2r(pos->b)), cb = cos(d2r(pos->b)); + const double sc = sin(d2r(pos->c)), cc = cos(d2r(pos->c)); + const double x = pos->tran.x, y = pos->tran.y; + const double R = pivot_length + pos->w; + int j, a; + + (void)joints; + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + // kinematicsInverse() with the polar form expanded: rotating (x, y) + // by -c is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the + // B and U corrections are what they are written as + jac[0][0] = cc; + jac[0][1] = sc; + jac[0][4] = (con * R * cb - pos->u * sb) * k; + jac[0][5] = (-x * sc + y * cc) * k; + jac[0][6] = cb; + jac[0][8] = con * sb; + + jac[1][0] = -sc; + jac[1][1] = cc; + jac[1][5] = (-x * cc - y * sc) * k; + jac[1][7] = 1; + + jac[2][2] = 1; + jac[2][4] = (-R * sb - con * pos->u * cb) * k; + jac[2][6] = -con * sb; + jac[2][8] = cb; + + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -121,6 +165,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 0cb4b5eb7aa..6f77bfd92ad 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -299,6 +299,59 @@ int xyzacKinematicsToolFrame(const double *joints, return 0; } // xyzacKinematicsToolFrame() +int xyzacKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)joints; + (void)iflags; + const double x_rot_point = hal_get_real(haldata->x_rot_point); + const double y_rot_point = hal_get_real(haldata->y_rot_point); + const double z_rot_point = hal_get_real(haldata->z_rot_point); + const double dy = hal_get_real(haldata->y_offset); + const double dt = hal_get_real(haldata->tool_offset); + const double dz = hal_get_real(haldata->z_offset) + dt; + const double sa = sin(pos->a*TO_RAD), ca = cos(pos->a*TO_RAD); + const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); + const double X = pos->tran.x - x_rot_point; + const double Y = pos->tran.y - y_rot_point; + const double Z = pos->tran.z - z_rot_point; + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } + } + + // the computed position P of xyzacKinematicsInverse(), differentiated: + // its coefficients for x, y and z, and the same expressions with the + // rotation taken a quarter turn on for a and for c + dP[0][0] = cc; + dP[0][1] = con * sc; + dP[0][5] = (-sc*X + con*cc*Y) * TO_RAD; + + dP[1][0] = - con * sc * ca; + dP[1][1] = cc * ca; + dP[1][2] = con * sa; + dP[1][3] = (con*sc*sa*X - cc*sa*Y + con*ca*Z + sa*dy - con*ca*dz) * TO_RAD; + dP[1][5] = (-con*cc*ca*X - sc*ca*Y) * TO_RAD; + + dP[2][0] = sc * sa; + dP[2][1] = - con * cc * sa; + dP[2][2] = ca; + dP[2][3] = (sc*ca*X - con*cc*ca*Y - sa*Z + con*ca*dy + sa*dz) * TO_RAD; + dP[2][5] = (cc*sa*X + con*sc*sa*Y) * TO_RAD; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxes(trtfuncs_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzacKinematicsJacobian() + int xyzbcKinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -443,3 +496,55 @@ int xyzbcKinematicsToolFrame(const double *joints, *rot = TOOL_FRAME_SPINDLE; return 0; } // xyzbcKinematicsToolFrame() + +int xyzbcKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)joints; + (void)iflags; + const double x_rot_point = hal_get_real(haldata->x_rot_point); + const double y_rot_point = hal_get_real(haldata->y_rot_point); + const double z_rot_point = hal_get_real(haldata->z_rot_point); + const double dx = hal_get_real(haldata->x_offset); + const double dt = hal_get_real(haldata->tool_offset); + const double dz = hal_get_real(haldata->z_offset) + dt; + const double sb = sin(pos->b*TO_RAD), cb = cos(pos->b*TO_RAD); + const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); + const double X = pos->tran.x - x_rot_point; + const double Y = pos->tran.y - y_rot_point; + const double Z = pos->tran.z - z_rot_point; + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } + } + + // see the comment in xyzacKinematicsJacobian(); dpx and dpz of the + // inverse depend on b as well + dP[0][0] = cc * cb; + dP[0][1] = con * sc * cb; + dP[0][2] = - con * sb; + dP[0][4] = (-cc*sb*X - con*sc*sb*Y - con*cb*Z + sb*dx + con*cb*dz) * TO_RAD; + dP[0][5] = (-sc*cb*X + con*cc*cb*Y) * TO_RAD; + + dP[1][0] = - con * sc; + dP[1][1] = cc; + dP[1][5] = (-con*cc*X - sc*Y) * TO_RAD; + + dP[2][0] = con * cc * sb; + dP[2][1] = sc * sb; + dP[2][2] = cb; + dP[2][4] = (con*cc*cb*X + sc*cb*Y - sb*Z - con*cb*dx + sb*dz) * TO_RAD; + dP[2][5] = (-con*sc*sb*X + cc*sb*Y) * TO_RAD; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxes(trtfuncs_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzbcKinematicsJacobian() diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 504f177e9ee..58c92a8a1df 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -41,6 +41,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(1, xyzacKinematicsWorkFrame, xyzacKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(1, xyzacKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc @@ -49,6 +50,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, xyzacKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(0, xyzacKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 6915099c832..68518d91537 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -41,6 +41,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(1, xyzbcKinematicsWorkFrame, xyzbcKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(1, xyzbcKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc @@ -49,6 +50,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, xyzbcKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(0, xyzbcKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; From add1d106d97302987ad2fd6c6cd834d65f57c774 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 29/58] corexykins, rotatekins, rosekins, matrixkins, millturn, userkins: supply the Jacobian The belt sum and difference, the rotation and its quarter turn, the polar radius and angle, the calibration matrix itself, and the two templates' joint to axis assignments. These are the short ones; they are here so that no module in the tree answers by differencing when its inverse is a few lines. --- src/emc/kinematics/corexykins.c | 20 +++++++++++++++++++ src/emc/kinematics/rosekins.c | 24 ++++++++++++++++++++++ src/emc/kinematics/rotatekins.c | 24 ++++++++++++++++++++++ src/hal/components/matrixkins.comp | 28 ++++++++++++++++++++++++++ src/hal/components/millturn.comp | 32 ++++++++++++++++++++++++++++++ src/hal/components/userkins.comp | 22 ++++++++++++++++++++ 6 files changed, 150 insertions(+) diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index f592ff52e9f..473a2ceede1 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -49,6 +49,25 @@ int kinematicsInverse(const EmcPose *pos return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + int j, a; + (void)joints; + (void)pos; + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // the two belt motors each carry x and y, in opposite senses for y + jac[0][0] = 1; jac[0][1] = 1; + jac[1][0] = 1; jac[1][1] = -1; + for (j = 2; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + int kinematicsHome(EmcPose *world ,double *joint ,KINEMATICS_FORWARD_FLAGS *fflags @@ -65,6 +84,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index adfe763a33a..9f73fbc3f9d 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -26,6 +26,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); #ifndef hypot @@ -112,6 +113,29 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double x = pos->tran.x, y = pos->tran.y; + double r2 = x*x + y*y; + double r = sqrt(r2); + int j, a; + (void)joints; + (void)iflags; + // on the axis the angle is undefined and its rate unbounded + if (r2 <= 0) { return -1; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + jac[0][0] = x/r; jac[0][1] = y/r; + jac[1][2] = 1; + jac[2][0] = -y/r2 * TO_DEG; + jac[2][1] = x/r2 * TO_DEG; + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 838c9178154..b5b648b4b38 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -60,6 +60,29 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double c_rad = pos->c*M_PI/180; + double cc = cos(c_rad), sc = sin(c_rad); + int j, a; + (void)joints; + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // the inverse above, differentiated: the rotation itself for x and y, + // and the rotated point turned a quarter turn for c + jac[0][0] = cc; jac[0][1] = -sc; + jac[0][5] = (-pos->tran.x*sc - pos->tran.y*cc) * (M_PI/180); + jac[1][0] = sc; jac[1][1] = cc; + jac[1][5] = ( pos->tran.x*cc - pos->tran.y*sc) * (M_PI/180); + for (j = 2; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + /* implemented for these kinematics as giving joints preference */ int kinematicsHome(EmcPose * world, double *joint, @@ -81,6 +104,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); int comp_id; diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index aac6c04d913..8bf76899c8e 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -229,6 +229,7 @@ error: KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() @@ -321,3 +322,30 @@ int kinematicsInverse(const EmcPose * pos, return 0; } + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + // the inverse is the calibration matrix itself, so its derivative is + // that matrix, and the pass-through axes are ones + jac[0][0] = hal_get_real(haldata->C_xx); + jac[0][1] = hal_get_real(haldata->C_xy); + jac[0][2] = hal_get_real(haldata->C_xz); + jac[1][0] = hal_get_real(haldata->C_yx); + jac[1][1] = hal_get_real(haldata->C_yy); + jac[1][2] = hal_get_real(haldata->C_yz); + jac[2][0] = hal_get_real(haldata->C_zx); + jac[2][1] = hal_get_real(haldata->C_zy); + jac[2][2] = hal_get_real(haldata->C_zz); + for (r = 3; r < 9; r++) { jac[r][r] = 1; } + return 0; +} diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 62d5980ed29..e6814434b72 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -99,6 +99,7 @@ EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); static rtapi_u32 switchkins_type; @@ -214,3 +215,34 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + // the derivative of kinematicsInverse() for each type: which joint + // follows which pose coordinate, and in which sense + switch (switchkins_type) { + case 0: + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + jac[3][3] = 1; + break; + case 1: + jac[2][0] = 1; + jac[1][1] = -1; + jac[0][2] = 1; + jac[3][3] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index a7af5d29a75..ac0c003369d 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -128,6 +128,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() @@ -194,3 +195,24 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + // How each joint responds to each pose coordinate, the derivative of + // kinematicsInverse(): for this template joint 0 follows x, joint 1 + // follows y and joint 2 follows z, each one for one. See kinematics.h. + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + return 0; +} // kinematicsJacobian() From dab0317958a187b19fd784bc9eee3db8f88d3f6d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 30/58] xyzab_tdr_kins, xyzacb_trsrn, xyzbca_trsrn: supply the Jacobian The dual rotary table is its TCP inverse differentiated: the rotation matrix for the linear columns, and each term with A or B advanced a quarter turn for the rotary ones. The nutating heads are differentiated the same way, term by term through the secondary angle's r, s and t and the primary angle's sine and cosine. Their TCP inverse reads the rotary angles from the joint argument rather than from the pose, the two being the same numbers once a move is done; the derivative is taken against the pose, which is what a consumer multiplies by, and is the coupling the machine has. The TOOL type takes its angles from pins, so its inverse is linear in the pose and its rows are the coefficients. --- src/hal/components/xyzab_tdr_kins.comp | 62 +++++++++++ src/hal/components/xyzacb_trsrn.comp | 136 +++++++++++++++++++++++++ src/hal/components/xyzbca_trsrn.comp | 136 +++++++++++++++++++++++++ 3 files changed, 334 insertions(+) diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 2d7a71b966d..a040e757e12 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -94,6 +94,7 @@ EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); static rtapi_u32 switchkins_type; @@ -255,3 +256,64 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (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 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 sa = sin(pos->a*TO_RAD); + double ca = cos(pos->a*TO_RAD); + double sb = sin(pos->b*TO_RAD); + double cb = cos(pos->b*TO_RAD); + 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; + int r, c; + + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + + switch (switchkins_type) { + case 0: // ====================== IDENTITY kinematics JACOBIAN ==================== + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + jac[3][3] = 1; + jac[4][4] = 1; + break; + case 1: // ========================= TCP kinematics JACOBIAN ====================== + // the TCP inverse above differentiated: its coefficients of + // qx, qy and qz for the linear columns, and the same terms + // with a or b advanced a quarter turn for the rotary columns + jac[0][0] = cb; + jac[0][1] = sa*sb; + jac[0][2] = -ca*sb; + jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; + jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; + + jac[1][1] = ca; + jac[1][2] = sa; + jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; + + jac[2][0] = sb; + jac[2][1] = -sa*cb; + jac[2][2] = ca*cb; + jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; + jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 4efedcae6ce..efe25aa1e46 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -90,6 +90,7 @@ EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); @@ -545,3 +546,138 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + + // the same geometry as kinematicsInverse(), read the same way + double Ly = hal_get_real(haldata->y_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double Dt = hal_get_real(haldata->tool_offset_z); + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + + // The TCP inverse reads the rotary angles from its joint argument, + // where the machine is, and its own pose words for the same angles + // are the same numbers once the move is done. Its derivative is taken + // against the pose, which is what a consumer multiplies by. + double Sw = sin(pos->a*TO_RAD); + double Cw = cos(pos->a*TO_RAD); + double Ss = 0, Cs = 0, Sp = 0, Cp = 0; + double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; + double dSp = 0, dCp = 0; + + double Qy = pos->tran.y; + double Qz = pos->tran.z; + double Ay, Az; // the two lever arms the table turns about + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + switch (switchkins_type) { + + case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== + for (R = 0; R < 6; R++) { jac[R][R] = 1; } + break; + + case 1: // ========================= TCP kinematics JACOBIAN + Ss = sin(pos->b*TO_RAD); + Cs = cos(pos->b*TO_RAD); + Sp = sin(pos->c*TO_RAD); + Cp = cos(pos->c*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + dSs = Cs*TO_RAD; + dr = -Ss*Cv*Cv*TO_RAD; + ds = -Ss*Sv*Sv*TO_RAD; + dt_ = Sv*Cv*Ss*TO_RAD; + dCvSs = Cv*dSs; + dSvSs = Sv*dSs; + dSp = Cp*TO_RAD; + dCp = -Sp*TO_RAD; + + Ay = Dray + Dy + Ly - Qy; + Az = Draz + Dt + Lz - Qz; + + // j[0]: Qx plus terms in the head angles only + jac[0][0] = 1; + jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; + jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx + - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; + + // j[1]: -Cw*Ay - Az*Sw plus head terms + jac[1][1] = Cw; + jac[1][2] = Sw; + jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; + jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; + jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Ly; + + // j[2]: -Cw*Az + Ay*Sw plus head terms + jac[2][1] = -Sw; + jac[2][2] = Cw; + jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; + jac[2][4] = (Dt + Lz)*ds + Ly*dt_; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + + case 2: // ========================= TOOL kinematics JACOBIAN + // the head angles come from pins, so the inverse is linear in + // the pose and the rows are its coefficients + Ss = sin(theta_2*TO_RAD); + Cs = cos(theta_2*TO_RAD); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[0][2] = (Cp*SvSs - Sp*t); + + jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[1][2] = (Sp*SvSs + Cp*t); + + jac[2][0] = -(Ctc*SvSs - Stc*t); + jac[2][1] = (Stc*SvSs + Ctc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 763b2801c33..f844a422953 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -92,6 +92,7 @@ EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); @@ -550,3 +551,138 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + + // the same geometry as kinematicsInverse(), read the same way + double Lx = hal_get_real(haldata->x_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double Dt = hal_get_real(haldata->tool_offset_z); + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + + // The TCP inverse reads the rotary angles from its joint argument, + // where the machine is, and its own pose words for the same angles + // are the same numbers once the move is done. Its derivative is taken + // against the pose, which is what a consumer multiplies by. + double Sw = sin(pos->b*TO_RAD); + double Cw = cos(pos->b*TO_RAD); + double Ss = 0, Cs = 0, Sp = 0, Cp = 0; + double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; + double dSp = 0, dCp = 0; + + double Qx = pos->tran.x; + double Qz = pos->tran.z; + double Ax, Az; // the two lever arms the table turns about + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + switch (switchkins_type) { + + case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== + for (R = 0; R < 6; R++) { jac[R][R] = 1; } + break; + + case 1: // ========================= TCP kinematics JACOBIAN + Ss = sin(pos->a*TO_RAD); + Cs = cos(pos->a*TO_RAD); + Sp = sin(pos->c*TO_RAD); + Cp = cos(pos->c*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + dSs = Cs*TO_RAD; + dr = -Ss*Cv*Cv*TO_RAD; + ds = -Ss*Sv*Sv*TO_RAD; + dt_ = Sv*Cv*Ss*TO_RAD; + dCvSs = Cv*dSs; + dSvSs = Sv*dSs; + dSp = Cp*TO_RAD; + dCp = -Sp*TO_RAD; + + Ax = Drax + Dx + Lx - Qx; + Az = Draz + Dt + Lz - Qz; + + // j[0]: -Cw*Ax + Az*Sw plus head terms + jac[0][0] = Cw; + jac[0][2] = -Sw; + jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; + jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; + jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Lx; + + // j[1]: Qy plus head terms + jac[1][1] = 1; + jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; + jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy + + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; + + // j[2]: -Cw*Az - Ax*Sw plus head terms + jac[2][0] = Sw; + jac[2][2] = Cw; + jac[2][3] = (Dt + Lz)*ds + Lx*dt_; + jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + + case 2: // ========================= TOOL kinematics JACOBIAN + // the head angles come from pins, so the inverse is linear in + // the pose and the rows are its coefficients + Ss = sin(theta_2*TO_RAD); + Cs = cos(theta_2*TO_RAD); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[0][2] = (Sp*SvSs + Cp*t); + + jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[1][2] = -(Cp*SvSs - Sp*t); + + jac[2][0] = (Stc*SvSs + Ctc*t); + jac[2][1] = (Ctc*SvSs - Stc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + } + return 0; +} // kinematicsJacobian() From 19a1e7a9ffd14f46be581ea8a8ac03f252d7bdc0 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 31/58] tripodkins, lineardeltakins, rotarydeltakins, genhexkins, pentakins: supply the Jacobian A strut or rod changes length by the component of its moving end's motion along it, so the rows of the parallel machines are unit vectors and moments rather than differentiated formulas. The tripod's rows are the strut directions; the linear delta's are the rod directions scaled by the rise; the rotary delta's follow from the foot staying a shin from each knee, so the foot and the knee agree along the leg. The hexapod's rows are the ones its own Newton step already builds, with the rotary columns taken through the matrix that carries roll, pitch and yaw rates to the angular velocity; with a screw lead set, whose correction is a function of the pose too, it falls back to differencing. The pentapod differentiates InvKins() the same way, in effector coordinates. --- src/emc/kinematics/genhexkins.c | 70 ++++++++++++++++++++++++++ src/emc/kinematics/lineardeltakins.c | 27 ++++++++++ src/emc/kinematics/pentakins.c | 75 ++++++++++++++++++++++++++++ src/emc/kinematics/rotarydeltakins.c | 53 ++++++++++++++++++++ src/emc/kinematics/tripodkins.c | 32 ++++++++++++ 5 files changed, 257 insertions(+) diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 3cddd9a72bc..a9210b6d917 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -544,6 +544,75 @@ static int genhexKinematicsInverse(const EmcPose * pos, return 0; } //genhexKinematicsInverse() +/************************ genhexKinematicsJacobian() ***********************/ +/* A strut length changes by the component of its platform end's motion + along the strut. That end moves with the platform, dP + w x (R a), so + the row for strut i is [u_i, (R a_i x u_i) . E] with u_i the unit strut + vector and E the matrix taking the rates of the roll, pitch and yaw + words to the angular velocity w for R = Rz(c) Ry(b) Rx(a). The forward + kinematics builds the same rows for its Newton step, in radians. */ + +static int genhexKinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + PmCartesian aw, RMatrix_a, strut, u, moment; + PmRotationMatrix RMatrix; + PmRpy rpy; + PmCartesian E[3]; + double sb, cb, sc, cc; + int i, j, col, m; + + genhex_read_hal_pins(); + + /* the screw lead correction is a function of the pose too, and this + does not differentiate it; difference the inverse instead */ + if (hal_get_real(haldata->screw_lead) != 0.0) { + return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS, + joints, pos, iflags, jac); + } + + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (col = 0; col < EMCMOT_MAX_AXIS; col++) { jac[j][col] = 0; } + } + + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = pos->c * PM_PI / 180.0; + pmRpyMatConvert(&rpy, &RMatrix); + + /* w = E [da db dc]: the roll axis carried by pitch and yaw, the pitch + axis carried by yaw, and the yaw axis fixed */ + sb = sin(rpy.p); cb = cos(rpy.p); + sc = sin(rpy.y); cc = cos(rpy.y); + E[0].x = cb*cc; E[0].y = cb*sc; E[0].z = -sb; + E[1].x = -sc; E[1].y = cc; E[1].z = 0; + E[2].x = 0; E[2].y = 0; E[2].z = 1; + + for (i = 0; i < NUM_STRUTS; i++) { + double len; + + pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); + pmCartCartSub(&aw, &b[i], &strut); + pmCartMag(&strut, &len); + if (len <= 0) { return -1; } + pmCartScalMult(&strut, 1.0/len, &u); + pmCartCartCross(&RMatrix_a, &u, &moment); + + jac[i][0] = u.x; + jac[i][1] = u.y; + jac[i][2] = u.z; + for (m = 0; m < 3; m++) { + double dot; + pmCartCartDot(&moment, &E[m], &dot); + jac[i][3+m] = dot * PM_PI / 180.0; + } + } + return 0; +} // genhexKinematicsJacobian() + // HAL pin initializaion values. In small arrays so we can easily // address them in the pin creation loop. static const rtapi_real init_basex[NUM_STRUTS] = { @@ -701,6 +770,7 @@ int switchkinsSetup(kparms* kp, *kset0 = genhexKinematicsSetup; *kfwd0 = genhexKinematicsForward; *kinv0 = genhexKinematicsInverse; + switchkinsRegisterJacobian(0, genhexKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 353e9234562..541643fef74 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -48,6 +48,32 @@ int kinematicsInverse(const EmcPose *pos, double *joints, return kinematics_inverse(pos, joints); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; + int i, j, a; + (void)iflags; + set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // each carriage is the platform height plus the rise of its rod, and + // the rise changes with the horizontal offset from the tower + for (i = 0; i < 3; i++) { + double tx = (i == 0) ? Ax : (i == 1) ? Bx : Cx; + double ty = (i == 0) ? Ay : (i == 1) ? By : Cy; + double rise = joints[i] - z; + if (rise <= 0) { return -1; } + jac[i][0] = (tx - x)/rise; + jac[i][1] = (ty - y)/rise; + jac[i][2] = 1; + } + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -85,4 +111,5 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 18487be3134..f8415b4112c 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -399,6 +399,80 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + PmRotationMatrix R; + PmRpy rpy; + PmCartesian P, d, xyz, wa, wb, dxyz[5]; + int i, j, a, col; + + (void)joints; + (void)iflags; + pentakins_read_hal_pins(); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + /* InvKins() differentiated. The effector end of each strut is found in + effector coordinates as xyz = R^T (b - P) with R = Ry(b) Rx(a), so a + pose translation moves it by -R^T and a pose rotation about w moves + it by -R^T (w x (b - P)); the strut length is then the distance from + that point to the strut's pivot circle of radius ra at height za. */ + P = pos->tran; + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = 0; + pmRpyMatConvert(&rpy, &R); + + /* rotation axes for a and b, in world coordinates */ + wa.x = cos(rpy.p); wa.y = 0; wa.z = -sin(rpy.p); + wb.x = 0; wb.y = 1; wb.z = 0; + + for (i = 0; i < NUM_STRUTS; i++) { + double rho, A, B, len; + + pmCartCartSub(&b[i], &P, &d); + /* R^T d, written out since pmMatCartMult applies R */ + xyz.x = R.x.x*d.x + R.x.y*d.y + R.x.z*d.z; + xyz.y = R.y.x*d.x + R.y.y*d.y + R.y.z*d.z; + xyz.z = R.z.x*d.x + R.z.y*d.y + R.z.z*d.z; + + /* d xyz / d pose, one PmCartesian per pose column x y z a b */ + for (col = 0; col < 3; col++) { + /* -R^T e_col, which is minus row col of R^T, i.e. minus column + col of R read as a row of R^T */ + PmCartesian e = {0, 0, 0}, w; + if (col == 0) e.x = 1; else if (col == 1) e.y = 1; else e.z = 1; + w.x = -(R.x.x*e.x + R.x.y*e.y + R.x.z*e.z); + w.y = -(R.y.x*e.x + R.y.y*e.y + R.y.z*e.z); + w.z = -(R.z.x*e.x + R.z.y*e.y + R.z.z*e.z); + dxyz[col] = w; + } + for (col = 3; col < 5; col++) { + PmCartesian cr, w; + pmCartCartCross(col == 3 ? &wa : &wb, &d, &cr); + w.x = -(R.x.x*cr.x + R.x.y*cr.y + R.x.z*cr.z) * (PM_PI/180.0); + w.y = -(R.y.x*cr.x + R.y.y*cr.y + R.y.z*cr.z) * (PM_PI/180.0); + w.z = -(R.z.x*cr.x + R.z.y*cr.y + R.z.z*cr.z) * (PM_PI/180.0); + dxyz[col] = w; + } + + rho = sqrt(sqr(xyz.x) + sqr(xyz.y)); + A = xyz.z - za[i]; + B = rho - ra[i]; + len = sqrt(sqr(A) + sqr(B)); + if (len <= 0 || rho <= 0) { return -1; } + for (col = 0; col < 5; col++) { + jac[i][col] = (A*dxyz[col].z + + B*(xyz.x*dxyz[col].x + xyz.y*dxyz[col].y)/rho) / len; + } + } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -408,6 +482,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 8c83ebdec4f..a2f52c10c1c 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -51,6 +51,58 @@ int kinematicsInverse(const EmcPose *pos, double *joints, return kinematics_inverse(pos, joints); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + int i, j, a; + (void)iflags; + set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // The foot stays a shin length from each knee, so along a leg the + // motion of the foot and the motion of the knee agree: + // (P - K) . dP = (P - K) . dK/dq dq + // K is the knee less the foot offset, written as kinematics_forward() + // writes it, and q the hip angle that swings it. + for (i = 0; i < 3; i++) { + double q = D2R(joints[i]); + double reach = platformradius - footradius + thighlength * cos(q); + double kx, ky, kz, dkx, dky, dkz, px, py, pz, denom; + switch (i) { + case 0: + kx = 0; ky = -reach; + dkx = 0; dky = thighlength * sin(q); + break; + case 1: + kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = -thighlength * sin(q) * 0.5 * sqrt(3); + dky = -thighlength * sin(q) * 0.5; + break; + default: + kx = -reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = thighlength * sin(q) * 0.5 * sqrt(3); + dky = -thighlength * sin(q) * 0.5; + break; + } + kz = -thighlength * sin(q); + dkz = -thighlength * cos(q); + px = pos->tran.x - kx; + py = pos->tran.y - ky; + pz = pos->tran.z - kz; + denom = (px*dkx + py*dky + pz*dkz) * (M_PI/180.); + // the shin at right angles to the thigh's swing: the knee cannot + // move the foot, so no finite hip rate follows the foot + if (fabs(denom) < 1e-12) { return -1; } + jac[i][0] = px/denom; + jac[i][1] = py/denom; + jac[i][2] = pz/denom; + } + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -92,4 +144,5 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index 990b7997297..c58d726dd46 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -218,6 +218,37 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } +int kinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + rtapi_real Bx = hal_get_real(haldata->bx); + rtapi_real Cx = hal_get_real(haldata->cx); + rtapi_real Cy = hal_get_real(haldata->cy); + /* the three strut base points, in the order of the joints */ + const double base[3][2] = { {0, 0}, {Bx, 0}, {Cx, Cy} }; + int i, j, a; + + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + /* a strut length changes by the component of the motion along the + strut, so each row is the unit vector from base to D */ + for (i = 0; i < 3; i++) { + double dx = pos->tran.x - base[i][0]; + double dy = pos->tran.y - base[i][1]; + double dz = pos->tran.z; + double len = joints[i]; + if (len <= 0) { return -1; } + jac[i][0] = dx/len; + jac[i][1] = dy/len; + jac[i][2] = dz/len; + } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -356,6 +387,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); From 8f10c54e52b8e1107fcbdc5d6793a4de262af166 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 32/58] scarakins, scorbot-kins: supply the Jacobian Both inverses are chains of a few closed form steps, and the derivative follows the chain: for the scara the squared reach fixes the elbow and the bearing less the outer arm's angle fixes the shoulder; for the scorbot the distance to the wrist fixes the isosceles triangle the shoulder and elbow make. Each declines at the poses where its own inverse has no derivative, the arm straight or folded. --- src/emc/kinematics/scarakins.c | 53 +++++++++++++++++++++++ src/emc/kinematics/scorbot-kins.c | 71 +++++++++++++++++++++++++++++++ 2 files changed, 124 insertions(+) diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 0ab0bd921de..2155263c55b 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -179,6 +179,58 @@ static int scaraKinematicsInverse(const EmcPose * world, return (0); } // scaraKinematicsInverse() +static int scaraKinematicsJacobian(const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)iflags; + rtapi_real D2 = hal_get_real(haldata->d2); + rtapi_real D4 = hal_get_real(haldata->d4); + rtapi_real D6 = hal_get_real(haldata->d6); + const double a3 = world->c * (PM_PI / 180); + const double q1 = joint[1] * (PM_PI / 180); + const double xt = world->tran.x - D6*cos(a3); + const double yt = world->tran.y - D6*sin(a3); + const double rsq = xt*xt + yt*yt; + /* gradients over (x, y, c) of the quantities the inverse builds */ + double d_xt[3] = { 1, 0, D6*sin(a3) * (PM_PI/180) }; + double d_yt[3] = { 0, 1, -D6*cos(a3) * (PM_PI/180) }; + double d_q1[3], d_q0[3], dphi_dq1; + int i, j, a; + + if (rsq <= 0 || fabs(sin(q1)) < 1e-12) { + /* the arm folded or straight out: the elbow rate is unbounded */ + return -1; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + /* rsq = D2^2 + D4^2 + 2 D2 D4 cos(q1), so q1 follows rsq; q0 is the + bearing of the end effector less the angle the outer arm subtends, + whose rate over q1 is (D2 D4 cos(q1) + D4^2) / rsq */ + dphi_dq1 = (D2*D4*cos(q1) + D4*D4) / rsq; + for (i = 0; i < 3; i++) { + double d_rsq = 2*xt*d_xt[i] + 2*yt*d_yt[i]; + d_q1[i] = -d_rsq / (2*D2*D4*sin(q1)); + d_q0[i] = (xt*d_yt[i] - yt*d_xt[i]) / rsq - dphi_dq1 * d_q1[i]; + } + + /* columns x, y, c; the rest of the pose does not reach these joints */ + for (i = 0; i < 3; i++) { + int col = (i == 2) ? 5 : i; + jac[0][col] = d_q0[i] * (180 / PM_PI); + jac[1][col] = d_q1[i] * (180 / PM_PI); + jac[3][col] = -(jac[0][col] + jac[1][col]); + } + jac[3][5] += 1; + jac[2][2] = -1; + jac[4][3] = 1; + jac[5][4] = 1; + return 0; +} // scaraKinematicsJacobian() + #define DEFAULT_D1 490 #define DEFAULT_D2 340 #define DEFAULT_D3 50 @@ -226,6 +278,7 @@ int switchkinsSetup(kparms* kp, *kset0 = scaraKinematicsSetup; *kfwd0 = scaraKinematicsForward; *kinv0 = scaraKinematicsInverse; + switchkinsRegisterJacobian(0, scaraKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index bd8868a063d..b7f933a3b71 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -294,6 +294,76 @@ int kinematicsInverse( } +int kinematicsJacobian( + const double *joints, + const EmcPose *pose, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags +) { + // kinematicsInverse() above, differentiated step by step in the same + // order, each quantity carried as its gradient over (x, y, z) + const double x = pose->tran.x, y = pose->tran.y; + const double rho2 = x*x + y*y; + const double rho = sqrt(rho2); + double r_cp, z_cp, dist, angle_to_cp, j1_angle, j1, z_j2, u; + double d_r_cp[3], d_z_cp[3], d_dist[3], d_angle[3], d_j1a[3], d_j1[3], d_j2[3]; + double q; + int i, j, a; + + (void)joints; + (void)iflags; + if (rho2 <= 0) { return -1; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + // j0 = atan2(y, x) + jac[0][0] = -y/rho2 * TO_DEG; + jac[0][1] = x/rho2 * TO_DEG; + + r_cp = rho - L0_HORIZONTAL_DISTANCE; + z_cp = pose->tran.z - L0_VERTICAL_DISTANCE; + d_r_cp[0] = x/rho; d_r_cp[1] = y/rho; d_r_cp[2] = 0; + d_z_cp[0] = 0; d_z_cp[1] = 0; d_z_cp[2] = 1; + + dist = sqrt(r_cp*r_cp + z_cp*z_cp); + if (dist <= 0 || dist >= 2*L1_LENGTH) { return -1; } + for (i = 0; i < 3; i++) { + d_dist[i] = (r_cp*d_r_cp[i] + z_cp*d_z_cp[i]) / dist; + } + + // the signed acos in the inverse is atan2(z_cp, r_cp) + angle_to_cp = TO_DEG * atan2(z_cp, r_cp); + for (i = 0; i < 3; i++) { + d_angle[i] = TO_DEG * (r_cp*d_z_cp[i] - z_cp*d_r_cp[i]) / (dist*dist); + } + + q = dist / (2*L1_LENGTH); + j1_angle = TO_DEG * acos(q); + for (i = 0; i < 3; i++) { + d_j1a[i] = -TO_DEG / sqrt(1 - q*q) * d_dist[i] / (2*L1_LENGTH); + } + + j1 = angle_to_cp + j1_angle; + for (i = 0; i < 3; i++) { + d_j1[i] = d_angle[i] + d_j1a[i]; + jac[1][i] = d_j1[i]; + } + + z_j2 = L1_LENGTH * sin(TO_RAD * j1); + u = (z_j2 - z_cp) / L2_LENGTH; + if (fabs(u) >= 1) { return -1; } + for (i = 0; i < 3; i++) { + double d_z_j2 = L1_LENGTH * cos(TO_RAD * j1) * TO_RAD * d_j1[i]; + d_j2[i] = -TO_DEG / sqrt(1 - u*u) * (d_z_j2 - d_z_cp[i]) / L2_LENGTH; + jac[2][i] = d_j2[i]; + } + + jac[3][3] = 1; + jac[4][4] = 1; + return 0; +} + KINEMATICS_TYPE kinematicsType(void) { return KINEMATICS_BOTH; } @@ -302,6 +372,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; From 7b17c92262e9e1635c83a12649053dca9b55a08c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 33/58] genserkins: supply the Jacobian from its geometric one compute_jinv() already gives radians of joint per unit of base frame twist. A pose word rate is not a twist: the roll, pitch and yaw rates reach the angular velocity through the matrix of the axes each one turns about, for the RPY convention go_rpy_mat_convert() uses. The Jacobian is that product, with the unit conversions and the unrotate coupling applied in the order the inverse applies them, and the u, v, w pass-through as ones. Having the conversion written once in the module is worth more than the closed form itself, since every consumer would otherwise guess it. --- src/emc/kinematics/genserfuncs.c | 107 +++++++++++++++++++++++++++++++ src/emc/kinematics/genserkins.c | 1 + src/emc/kinematics/genserkins.h | 5 ++ 3 files changed, 113 insertions(+) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 5600ab2be1f..d8432cdec3d 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -313,6 +313,113 @@ int genser_kin_jac_fwd(void *kins, return GO_RESULT_OK; } +/* The Jacobian in the terms of kinematics.h: joints in degrees per pose + word in EmcPose units, the derivative of genserKinematicsInverse(). + + compute_jinv() gives the geometric inverse Jacobian, radians of joint per + unit of base-frame twist. A pose word rate is not a twist: the roll, + pitch and yaw rates reach the angular velocity through E, the matrix of + the axes each one turns about, for the RPY convention of go_rpy_mat_convert, + R = Rz(yaw) Ry(pitch) Rx(roll). So + + dq/dp = unrotate . deg . Jinv . blockdiag(I, E . rad) + + with the unit conversions and the unrotate coupling applied in the order + the inverse applies them. */ +int genserKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)iflags; + genser_struct *genser = KINS_PTR; + GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); + GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); + go_pose T_L_0; + go_link linkout[GENSER_MAX_JOINTS] = {}; + go_real jest[GENSER_MAX_JOINTS]; + double E[3][3]; + double sb, cb, sc, cc; + int link, i, j, a, m, retval; + +#ifndef ULAPI + genser_kin_init(); + if (!genser_hal_inited) { + rtapi_print_msg(RTAPI_MSG_ERR, + "genserKinematicsJacobian: not initialized\n"); + return -1; + } +#endif + + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + // the kinematic joint angles, in radians and with the unrotate + // coupling removed, exactly as the forward prepares them + for (link = 0; link < genser->link_num; link++) { + rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + jest[link] = joint[link] * (PM_PI / 180); + if (link && unrotate) + jest[link] -= unrotate * jest[link-1]; + } + + go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); + go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); + + for (link = 0; link < genser->link_num; link++) { + retval = go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); + if (GO_RESULT_OK != retval) + return -1; + } + retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); + if (GO_RESULT_OK != retval) + return -1; + retval = compute_jinv(&Jfwd, &Jinv); + if (GO_RESULT_OK != retval) + return -1; // singular: no finite joint rate follows the pose + + // E columns: the roll axis carried by pitch and yaw, the pitch axis + // carried by yaw, and the yaw axis fixed + sb = sin(world->b * PM_PI / 180); cb = cos(world->b * PM_PI / 180); + sc = sin(world->c * PM_PI / 180); cc = cos(world->c * PM_PI / 180); + E[0][0] = cb*cc; E[1][0] = cb*sc; E[2][0] = -sb; + E[0][1] = -sc; E[1][1] = cc; E[2][1] = 0; + E[0][2] = 0; E[1][2] = 0; E[2][2] = 1; + + for (i = 0; i < genser->link_num; i++) { + // linear pose words: the twist column is the pose column, and the + // joint comes out in radians + for (a = 0; a < 3; a++) { + jac[i][a] = Jinv.el[i][a] * (180 / PM_PI); + } + // angular pose words: through E, radians of pose word per degree + // of pose word and degrees of joint per radian of joint cancel + for (m = 0; m < 3; m++) { + double s = 0; + for (a = 0; a < 3; a++) { s += Jinv.el[i][3+a] * E[a][m]; } + jac[i][3+m] = s; + } + } + + // the unrotate coupling, in link order as the inverse applies it + for (link = 1; link < genser->link_num; link++) { + rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + if (unrotate) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + jac[link][a] += unrotate * jac[link-1][a]; + } + } + } + + // uvw pass through as joints 6, 7, 8 + if (total_joints > 6) jac[6][6] = 1; + if (total_joints > 7) jac[7][7] = 1; + if (total_joints > 8) jac[8][8] = 1; + + return 0; +} // genserKinematicsJacobian() + /* main function called by emc2 for forward Kins */ int genserKinematicsForward(const double *joint, EmcPose * world, diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index 64fe55983e1..8c209413b28 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -66,6 +66,7 @@ int switchkinsSetup(kparms* kp, *kset0 = genserKinematicsSetup; *kfwd0 = genserKinematicsForward; *kinv0 = genserKinematicsInverse; + switchkinsRegisterJacobian(0, genserKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index 3aa0756fc5a..b74b826d2ec 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -142,6 +142,11 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); +extern int genserKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int genserKinematicsForward(const double *joint, EmcPose * world, const KINEMATICS_FORWARD_FLAGS * fflags, From e2e66a018ed4199db41ce1211e7c2ae3d7b574bd Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 34/58] tests: check the Jacobian of every module where it runs A realtime component loaded after the module under test, reaching it through the exported entry points, with a failed check failing the load. Every kinematics module in the tree, every switchkins type, both conventional-directions settings on the tables. Two checks, neither reusing the module's own answer. Against the forward: perturb one joint, difference the forward, multiply by the Jacobian, and expect that joint's unit vector; the forward is a separate piece of code from the inverse, so this catches a transposed matrix, a wrong sign, a wrong column and a wrong unit whichever way the module answered. Against the inverse: difference it here with a different step and compare entry by entry, which is the check for the gantry, whose forward is not one to one. Verified by mutation, in failed checks, one mutation per module or shared routine and none passing: shared differencing, step sign reversed: 3392 shared differencing, matrix transposed: 4224 identity, columns shifted (gantry run): 8 xyzac, one sign in the A column: 40 xyzbc, one entry transposed: 81 5axiskins, one sign in the B column: 30 maxkins, one sign: 15 corexykins, belt difference sign: 3 rotatekins, one entry transposed: 22 rosekins, one sign: 11 matrixkins, one entry transposed: 4 millturn, the turned Y sign: 2 xyzab_tdr_kins, one sign in the A column: 30 xyzacb_trsrn, one sign in the B column: 75 xyzacb_trsrn, one term of the A column dropped: 100 xyzbca_trsrn, one sign in the B column: 100 tripodkins, two components swapped: 8 lineardeltakins, one sign: 12 rotarydeltakins, knee derivative sign: 33 genhexkins, E transposed: 1939 genhexkins, rotary unit scaling dropped: 3456 pentakins, cross product reversed: 560 scarakins, elbow derivative sign: 1500 scorbot-kins, one sign: 32 genserkins, E transposed: 960 genserkins, linear unit scaling dropped: 768 genserkins, unrotate coupling sign: 64 The check against the forward is also a round trip test of each module, and it found four modules whose forward and inverse did not agree, fixed in the commits before this one. Two are left as they are and checked in the way that fits them: maxkins, whose forward and inverse disagree away from c = 0 and u = 0 and which a separate change addresses, is checked against its inverse; the nutating heads read their rotary angles from the joint argument of the inverse, so differencing the inverse about a pose cannot see the coupling, and they are checked against the forward. --- tests/kins-jacobian/checkresult | 4 + tests/kins-jacobian/jaccheck.c | 360 ++++++++++++++++++++++++++++++++ tests/kins-jacobian/skip | 4 + tests/kins-jacobian/test.sh | 176 ++++++++++++++++ 4 files changed, 544 insertions(+) create mode 100755 tests/kins-jacobian/checkresult create mode 100644 tests/kins-jacobian/jaccheck.c create mode 100755 tests/kins-jacobian/skip create mode 100755 tests/kins-jacobian/test.sh diff --git a/tests/kins-jacobian/checkresult b/tests/kins-jacobian/checkresult new file mode 100755 index 00000000000..b49a90b17c6 --- /dev/null +++ b/tests/kins-jacobian/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +[ "$(grep -c 'jacobian agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ + && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-jacobian/jaccheck.c b/tests/kins-jacobian/jaccheck.c new file mode 100644 index 00000000000..0e5501f6943 --- /dev/null +++ b/tests/kins-jacobian/jaccheck.c @@ -0,0 +1,360 @@ +/* Check a kinematics module's Jacobian where it runs in service. + * + * Loaded after the module under test, so kinematicsForward(), + * kinematicsInverse() and kinematicsJacobian() resolve to it. A + * failed check fails the load, and a failed load fails the test. + * + * Two checks, neither of which reuses the module's own answer. + * + * Against the forward: perturb one joint, difference the forward to + * get how the pose responds, and multiply by the reported Jacobian. + * The result has to be that joint's unit vector, since the Jacobian + * is the derivative of the inverse and the two are inverse maps. The + * forward is a separate piece of code from the inverse, so this + * catches a transposed matrix, a wrong sign, a wrong column and a + * wrong unit, whether the module answered in closed form or by + * differencing. + * + * Against the inverse: difference the inverse here, with a different + * step, and compare entry by entry. This is the check for a machine + * whose forward is not one to one, the gantry with two joints on one + * letter, where the product above is not the identity. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("kinematics Jacobian checker"); + +static int joints = 3; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); + +static int types = -1; +RTAPI_MP_INT(types, "how many switchkins types to check, from 0; -1 for all the module has"); + +static int r1 = -1, r2 = -1, r3 = -1; +RTAPI_MP_INT(r1, "joint number of the first joint to sweep"); +RTAPI_MP_INT(r2, "joint number of the second joint to sweep, -1 for none"); +RTAPI_MP_INT(r3, "joint number of the third joint to sweep, -1 for none"); + +#define MAX_ANGLES 8 +#define NO_ANGLE 9999 +static int angles[MAX_ANGLES] = { NO_ANGLE, NO_ANGLE, NO_ANGLE, NO_ANGLE, + NO_ANGLE, NO_ANGLE, NO_ANGLE, NO_ANGLE }; +RTAPI_MP_ARRAY_INT(angles, MAX_ANGLES, "values each swept joint takes; default 0,30,-25,90,180"); + +static int base[EMCMOT_MAX_JOINTS] = { 10, 20, 30 }; +RTAPI_MP_ARRAY_INT(base, EMCMOT_MAX_JOINTS, "joint values before the sweep, from joint 0"); + +static int frompose = 0; +RTAPI_MP_INT(frompose, "1 to read base and the sweep as pose coordinates and take the joints from the inverse"); + +static char *check = "both"; +RTAPI_MP_STRING(check, "fwd, inv or both: which checks to run"); + +static int tolexp = 6; +RTAPI_MP_INT(tolexp, "tolerance for the checks is 10 to the minus this"); + +/* switchkins.h is not an exported header, and a module rejects a type + it does not have, so the loop only needs an upper bound */ +#define MAX_TYPES 9 + +#define FWD_STEP 1e-5 /* joint units, for differencing the forward */ +#define INV_STEP 2e-3 /* pose units, for differencing the inverse; not + the step kins_util.c uses, on purpose */ + +static int comp_id = -1; +static int failures; +static int poses; +static double tolerance = 1e-6; +static int do_fwd = 1, do_inv = 1; + +static void expect(int ok, const char *what, const double *j, int m, int n) +{ + char pose[160]; + int i, k = 0; + + if (ok) { return; } + for (i = 0; i < joints && k < (int)sizeof(pose) - 12; i++) { + k += rtapi_snprintf(pose + k, sizeof(pose) - k, "%s%.4g", + i ? "," : "", j[i]); + } + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: FAIL %s [%d][%d] at [%s]\n", + what, m, n, pose); + failures++; +} + +static double pose_coord(const EmcPose *p, int a) +{ + switch (a) { + 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 pose_add(EmcPose *p, int a, double d) +{ + switch (a) { + case 0: p->tran.x += d; break; + case 1: p->tran.y += d; break; + case 2: p->tran.z += d; break; + case 3: p->a += d; break; + case 4: p->b += d; break; + case 5: p->c += d; break; + case 6: p->u += d; break; + case 7: p->v += d; break; + default: p->w += d; break; + } +} + +/* how the pose responds to joint m: column m of the forward's derivative. + A forward that iterates starts from the pose it is handed, so both + calls start from the pose the joints are known to reach. */ +static int fwd_column(const double *j, int m, KINEMATICS_FORWARD_FLAGS ff, + const EmcPose *near, double *col) +{ + double t[EMCMOT_MAX_JOINTS]; + EmcPose lo = *near, hi = *near; + KINEMATICS_INVERSE_FLAGS inf = 0; + int a; + + memcpy(t, j, sizeof(t)); + + t[m] = j[m] - FWD_STEP; + if (kinematicsForward(t, &lo, &ff, &inf)) { return -1; } + t[m] = j[m] + FWD_STEP; + if (kinematicsForward(t, &hi, &ff, &inf)) { return -1; } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + col[a] = (pose_coord(&hi, a) - pose_coord(&lo, a)) / (2 * FWD_STEP); + } + return 0; +} + +/* near is where the pose is expected to be, for a forward that iterates + from the pose it is handed; zero where nothing better is known */ +static void check_pose(const double *j, const EmcPose *near) +{ + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double col[EMCMOT_MAX_AXIS]; + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + EmcPose world = *near, p; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + int m, n, a; + + m = kinematicsForward(j, &world, &ff, &inf); + if (m) { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: forward started from [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g]" + " and left [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g]\n", + near->tran.x, near->tran.y, near->tran.z, near->a, near->b, near->c, + world.tran.x, world.tran.y, world.tran.z, world.a, world.b, world.c); + expect(0, "forward kinematics", j, m, -1); + return; + } + poses++; + + if (kinematicsJacobian(j, &world, jac, &inf)) { + /* say what the inverse makes of the same pose, since a module + that differences its inverse declines when that does not come + back to the joints it was given */ + memcpy(qp, j, sizeof(qp)); + if (kinematicsInverse(&world, qp, &inf, &ff)) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: inverse fails at the pose\n"); + } else { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: inverse gives [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g] flags %lu\n", + qp[0], qp[1], qp[2], qp[3], qp[4], qp[5], inf); + } + expect(0, "jacobian declined", j, -1, -1); + return; + } + + /* rows the module has no joint for stay zero */ + for (m = joints; m < EMCMOT_MAX_JOINTS; m++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + expect(jac[m][a] == 0, "row past the joint count", j, m, a); + } + } + + if (do_fwd) { + for (m = 0; m < joints; m++) { + if (fwd_column(j, m, ff, &world, col)) { + expect(0, "forward kinematics near the pose", j, m, -1); + return; + } + for (n = 0; n < joints; n++) { + double s = 0; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { s += jac[n][a] * col[a]; } + expect(fabs(s - (m == n ? 1.0 : 0.0)) < tolerance, + "jacobian times forward column", j, n, m); + } + } + } + + if (do_inv) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p = world; + memcpy(qp, j, sizeof(qp)); + memcpy(qm, j, sizeof(qm)); + pose_add(&p, a, INV_STEP); + if (kinematicsInverse(&p, qp, &inf, &ff)) { + expect(0, "inverse kinematics near the pose", j, -1, a); + return; + } + pose_add(&p, a, -2 * INV_STEP); + if (kinematicsInverse(&p, qm, &inf, &ff)) { + expect(0, "inverse kinematics near the pose", j, -1, a); + return; + } + for (n = 0; n < joints; n++) { + double d = (qp[n] - qm[n]) / (2 * INV_STEP); + expect(fabs(d - jac[n][a]) < tolerance * (1 + fabs(d)), + "jacobian against the inverse", j, n, a); + } + } + } +} + +int rtapi_app_main(void) +{ + double j[EMCMOT_MAX_JOINTS]; + int angles_n; + int a, b, c, t, i; + int checked = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: joints=%d\n", joints); + return -1; + } + /* the list given ends at the first untouched entry; none given means + the quarter and half turns where a sine changes sign or a cosine + vanishes, and the values in between */ + if (angles[0] == NO_ANGLE) { + static const int usual[] = { 0, 30, -25, 90, 180 }; + for (i = 0; i < (int)(sizeof(usual)/sizeof(usual[0])); i++) { angles[i] = usual[i]; } + } + for (angles_n = 0; angles_n < MAX_ANGLES; angles_n++) { + if (angles[angles_n] == NO_ANGLE) { break; } + } + for (tolerance = 1, i = 0; i < tolexp; i++) { tolerance *= 0.1; } + do_fwd = !strcmp(check, "fwd") || !strcmp(check, "both"); + do_inv = !strcmp(check, "inv") || !strcmp(check, "both"); + if (!do_fwd && !do_inv) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: check=%s\n", check); + return -1; + } + + comp_id = hal_init("jaccheck"); + if (comp_id < 0) { return comp_id; } + + if (kinematicsType() == 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: the module reports no type\n"); + hal_exit(comp_id); + return -1; + } + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { j[i] = base[i]; } + + /* A switchable module's first forward after load restarts an + iterating forward from a stored pose that is still zero, which + for a hexapod is the singular pose it cannot leave; motion's first + cycle takes that failure and carries on. Take it here. */ + if (kinematicsSwitchable()) { + double q[EMCMOT_MAX_JOINTS]; + EmcPose seed; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + ZERO_EMC_POSE(seed); + memcpy(q, j, sizeof(q)); + if (r1 >= 0) { q[r1] = angles[0]; } + if (r2 >= 0) { q[r2] = angles[0]; } + if (r3 >= 0) { q[r3] = angles[0]; } + if (frompose) { + for (i = 0; i < EMCMOT_MAX_AXIS; i++) { pose_add(&seed, i, q[i]); } + memset(q, 0, sizeof(q)); + kinematicsInverse(&seed, q, &inf, &ff); + } + kinematicsForward(q, &seed, &ff, &inf); + } + + /* every kinematics the module offers, since the answer is per type. + The module starts in type 0, and is not switched to it: a switch + restarts an iterating forward from a stored pose that is still + zero, which for a hexapod is the singular pose it cannot leave */ + for (t = 0; t < MAX_TYPES && (types < 0 || t < types); t++) { + if (kinematicsSwitchable() && t > 0 && kinematicsSwitch(t)) { break; } + checked++; + + for (a = 0; a < angles_n; a++) { + if (r1 >= 0) { j[r1] = angles[a]; } + for (b = 0; b < angles_n; b++) { + if (r2 >= 0) { j[r2] = angles[b]; } + for (c = 0; c < angles_n; c++) { + if (r3 >= 0) { j[r3] = angles[c]; } + if (frompose) { + /* base and sweep name a pose; the machine that + reaches it comes from the module's inverse */ + double q[EMCMOT_MAX_JOINTS]; + EmcPose want; + KINEMATICS_INVERSE_FLAGS inf = 0; + KINEMATICS_FORWARD_FLAGS ff = 0; + ZERO_EMC_POSE(want); + for (i = 0; i < EMCMOT_MAX_AXIS; i++) { pose_add(&want, i, j[i]); } + memset(q, 0, sizeof(q)); + if (kinematicsInverse(&want, q, &inf, &ff)) { + expect(0, "inverse kinematics at the base pose", j, -1, -1); + } else { + check_pose(q, &want); + } + } else { + EmcPose zero; + ZERO_EMC_POSE(zero); + check_pose(j, &zero); + } + if (r3 < 0) { break; } + } + if (r2 < 0) { break; } + } + if (r1 < 0) { break; } + } + + if (!kinematicsSwitchable()) { break; } + } + + if (failures) { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: %d check(s) failed over %d pose(s)\n", + failures, poses); + hal_exit(comp_id); + return -1; + } + + rtapi_print("jaccheck: jacobian agrees for %d kinematics type(s), %d pose(s)\n", + checked, poses); + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-jacobian/skip b/tests/kins-jacobian/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-jacobian/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-jacobian/test.sh b/tests/kins-jacobian/test.sh new file mode 100755 index 00000000000..d4dc02ba394 --- /dev/null +++ b/tests/kins-jacobian/test.sh @@ -0,0 +1,176 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install jaccheck.c >/dev/null + +# One hal file per module: they all define the same entry points, so +# only one can be loaded at a time. A run that leaves the sweep at its +# default takes each rotary through the quarter and half turns where a +# sine changes sign or a cosine vanishes; the arms and the parallel +# machines name their own, away from the poses they cannot hold. +# ONLY= in the environment runs the entries for that module alone +run() { + local hal + case "$1" in "${ONLY:-}"*) ;; *) return 0 ;; esac + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s\n' "$1" + printf '%s\n' "$2" + printf 'loadrt jaccheck %s\n' "$3" + } > "$hal" + echo "=== $1" + halrun -f "$hal" + rm -f "$hal" +} + +# identity, including a gantry: two joints on one letter is the case where +# the forward is not one to one, so it is checked against the inverse +run "trivkins coordinates=XYZ" "" "joints=3" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 check=inv" +run "trivkins coordinates=XYZABCUVW" "" "joints=9 r1=3 r2=5" +run "userkins" "" "joints=3" +run "millturn" "" "joints=4" + +# linear maps and one rotation +run "corexykins" "" "joints=9" +run "rotatekins" "" "joints=9 r1=5" +run "matrixkins" \ + "setp matrixkins.C_xy 0.02 +setp matrixkins.C_xz -0.01 +setp matrixkins.C_yx 0.03 +setp matrixkins.C_yz 0.015 +setp matrixkins.C_zx -0.02 +setp matrixkins.C_zy 0.01 +setp matrixkins.C_zz 1.001" \ + "joints=9" + +# tables and heads; offsets set so no term drops out. The forward and +# inverse of maxkins do not agree away from c = 0 and u = 0, which a +# separate fix addresses; until then its Jacobian, the derivative of the +# inverse, is checked against the inverse alone. +run "maxkins" \ + "setp maxkins.pivot-length 100" \ + "joints=9 r1=4 r2=5 base=10,20,30,0,0,0,7,0,3 check=inv" + +run "5axiskins coordinates=XYZBCW" "" "joints=6 r1=3 r2=4 base=10,20,30,0,0,5" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 r1=3 r2=4 base=10,20,30,0,0,5" + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +# and both with the rotation sense the chapter asks for +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.conventional-directions 1 +setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +# The nutating heads read their rotary angles from the joint argument of +# the inverse rather than from the pose, so differencing the inverse +# about a pose cannot see the coupling; the forward is the check here. +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400 +setp xyzacb_trsrn_kins.tool-offset-z 50 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 r1=3 r2=4 r3=5 check=fwd" + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400 +setp xyzbca_trsrn_kins.tool-offset-z 50 +setp xyzbca_trsrn_kins.pre-rot 0.3 +setp xyzbca_trsrn_kins.primary-angle 20 +setp xyzbca_trsrn_kins.secondary-angle 35" \ + "joints=6 r1=3 r2=4 r3=5 check=fwd" + +# polar +run "rosekins" "" "joints=3 r1=2 base=10,5,0 angles=30,-25,90,120" + +# arms. Straight or folded they are singular, so the sweep keeps clear +# of 0 and 180 on the elbow. genserkins iterates its inverse to a +# tolerance the differences would not see through, so it is checked +# against its forward only; pumakins and three21kins answer by differencing +# their own inverse and the forward is what proves the answer. +run "scarakins" "" "joints=6 r1=1 r2=3 r3=0 base=0,0,20,0,0,0 angles=30,-25,90,120,-60" +# scorbot's inverse returns the elbow-up arm, shoulder above elbow, so the +# poses have to be ones it can return: j1 above j2, and j2 within a quarter +# turn of level +run "scorbot-kins" "" "joints=5 r1=1 base=0,70,-20,0,0 angles=40,55,70,85" +run "scorbot-kins" "" "joints=5 r1=2 base=0,80,0,0,0 angles=-60,-30,0,20" +run "pumakins" "setp pumakins.D6 50" "joints=6 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70" +run "three21kins" "" "joints=6 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70" +run "genserkins" "" "joints=9 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70 check=fwd" +# and with a joint counted relative to the one before it +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70 check=fwd" + +# parallel machines. The struts cannot tilt the platform far, and the +# forward of the hexapod and the pentapod iterates to a tolerance, so the +# product check on those two is held to what that tolerance allows. The +# hexapod module runs its own forward for its GUI pins in every type, with +# whatever joint values that type has, and identity joint values are not +# strut lengths it can converge from; its identity types are the shared +# ones trivkins covers, so only its own type is checked. +run "tripodkins" \ + "setp tripodkins.Bx 2 +setp tripodkins.Cx 1 +setp tripodkins.Cy 2" \ + "joints=3 frompose=1 base=1,1,2" +run "lineardeltakins" "" "joints=9 frompose=1 base=20,30,-200" +run "rotarydeltakins" "" "joints=9 r1=0 r2=1 frompose=1 base=0,0,-12 angles=0,2,-3" +run "genhexkins" \ + "setp genhexkins.screw-lead 0" \ + "joints=6 r1=3 r2=4 r3=5 frompose=1 base=2,3,20 angles=0,5,-7,10 tolexp=3 types=1" +run "genhexkins" \ + "setp genhexkins.screw-lead 5" \ + "joints=6 r1=3 r2=4 r3=5 frompose=1 base=2,3,20 angles=0,5,-7,10 tolexp=3 types=1" +run "pentakins" "" "joints=5 r1=3 r2=4 frompose=1 base=10,20,0 angles=0,5,-7,10 tolexp=3" From 3706a9a8a60a0d180da6075ebad0d00a130eab83 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 35/58] 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 | 60 ++++++------------ src/emc/kinematics/switchkins.h | 11 +++- src/emc/kinematics/switchkins_main.c | 94 ++++++++++++++++++++++++++++ 4 files changed, 129 insertions(+), 44 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 e28db5f9a97..57f36ea9c14 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 @@ -384,12 +383,6 @@ int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) return 0; } // switchkinsRegisterToolFrameInverse() -//********************************************************************* -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); @@ -403,33 +396,23 @@ EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsRegisterJacobian); -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;} + 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;} // an identity type answers the tool frame the same way whichever module // asked for it, so supply it here rather than in every switchkinsSetup() @@ -445,7 +428,7 @@ int rtapi_app_main(void) } } - // the highest type provided by either route sets the count + // 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; } } @@ -484,11 +467,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]), @@ -502,8 +482,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); @@ -514,14 +494,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 c76355b0e00..df8dfbafa8c 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -34,14 +34,14 @@ 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); // called from switchkinsSetup() for each type that reports its frames; a type @@ -78,4 +78,11 @@ typedef int (*KJ)(const double *joint, // that does not gets the exact answer if it is an identity type, and // otherwise the generic differences of its own inverse. extern int switchkinsRegisterJacobian(int ktype, KJ kjac); + +// 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 f1e415809a2fd9b2aee685692fe538b1bb4e8f12 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 36/58] 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 | 1145 ++++++++++++++++++++++++ share/linuxcnc/switchkins.c | 502 +++++++++++ src/Makefile | 1 + src/emc/kinematics/switchkins.h | 8 +- src/hal/components/Submakefile | 13 +- src/hal/components/millturn.comp | 257 ++---- src/hal/components/xyzab_tdr_kins.comp | 371 +++----- src/hal/components/xyzacb_trsrn.comp | 608 ++++++------- src/hal/components/xyzbca_trsrn.comp | 610 ++++++------- 10 files changed, 2508 insertions(+), 1093 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..392cd046c46 --- /dev/null +++ b/share/linuxcnc/kins_util.c @@ -0,0 +1,1145 @@ +/* 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 +#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() + +const PmRotationMatrix TOOL_FRAME_SPINDLE = { + { 1, 0, 0}, // tool x + { 0, 1, 0}, // tool y + { 0, 0, 1} // tool axis +}; + +// half turn about tool x: reverses the tool axis and tool y, keeps tool x, +// and keeps the frame right-handed. Negating the tool axis on its own would +// leave a reflection, which is not a frame any machine can hold. +const PmRotationMatrix TOOL_FRAME_FLANGE = { + { 1, 0, 0}, + { 0, -1, 0}, + { 0, 0, -1} +}; + +int toolFrameIsProper(const PmRotationMatrix *m) +{ + const double c[3][3] = { + { m->x.x, m->y.x, m->z.x }, + { m->x.y, m->y.y, m->z.y }, + { m->x.z, m->y.z, m->z.z } + }; + double det; + int a, b, k; + + for (a = 0; a < 3; a++) { + for (b = a; b < 3; b++) { + double dot = 0; + for (k = 0; k < 3; k++) { dot += c[k][a] * c[k][b]; } + if (fabs(dot - (a == b ? 1.0 : 0.0)) > 1e-9) { return 0; } + } + } + + det = c[0][0] * (c[1][1]*c[2][2] - c[1][2]*c[2][1]) + - c[0][1] * (c[1][0]*c[2][2] - c[1][2]*c[2][0]) + + c[0][2] * (c[1][0]*c[2][1] - c[1][1]*c[2][0]); + + return fabs(det - 1.0) <= 1e-9; +} // toolFrameIsProper() + +int toolFrameApplyNative(PmRotationMatrix *rot, + const PmRotationMatrix *native) +{ + // rot holds the module's own frame, native the rotation relating it to + // the convention, so the answer is rot * native: the declared rotation is + // expressed in the module's frame, not in machine coordinates. + const double r[3][3] = { + { rot->x.x, rot->y.x, rot->z.x }, + { rot->x.y, rot->y.y, rot->z.y }, + { rot->x.z, rot->y.z, rot->z.z } + }; + const double n[3][3] = { + { native->x.x, native->y.x, native->z.x }, + { native->x.y, native->y.y, native->z.y }, + { native->x.z, native->y.z, native->z.z } + }; + double m[3][3]; + int a, b, k; + + if (!toolFrameIsProper(native)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "toolFrameApplyNative: declared rotation is not a proper rotation\n"); + return -1; + } + + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m[a][b] = 0; + for (k = 0; k < 3; k++) { m[a][b] += r[a][k] * n[k][b]; } + } + } + + rot->x.x = m[0][0]; rot->y.x = m[0][1]; rot->z.x = m[0][2]; + rot->x.y = m[1][0]; rot->y.y = m[1][1]; rot->z.y = m[1][2]; + rot->x.z = m[2][0]; rot->y.z = m[2][1]; rot->z.z = m[2][2]; + + return 0; +} // toolFrameApplyNative() + +int toolFrameInWork(const PmRotationMatrix *work, + const PmRotationMatrix *tool, + PmRotationMatrix *out) +{ + // transpose(work) * tool: both are given against the machine, and + // transposing the work frame turns "machine to work" out of "work to + // machine" without a general inverse, because a rotation is orthonormal + const double w[3][3] = { + { work->x.x, work->y.x, work->z.x }, + { work->x.y, work->y.y, work->z.y }, + { work->x.z, work->y.z, work->z.z } + }; + const double t[3][3] = { + { tool->x.x, tool->y.x, tool->z.x }, + { tool->x.y, tool->y.y, tool->z.y }, + { tool->x.z, tool->y.z, tool->z.z } + }; + double m[3][3]; + int a, b, k; + + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m[a][b] = 0; + for (k = 0; k < 3; k++) { m[a][b] += w[k][a] * t[k][b]; } + } + } + + out->x.x = m[0][0]; out->y.x = m[0][1]; out->z.x = m[0][2]; + out->x.y = m[1][0]; out->y.y = m[1][1]; out->z.y = m[1][2]; + out->x.z = m[2][0]; out->y.z = m[2][1]; out->z.z = m[2][2]; + + return 0; +} // toolFrameInWork() + +int identityKinematicsWorkFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)joints; + (void)fflags; + // nothing carries the work, so it stays square with the machine + *rot = TOOL_FRAME_SPINDLE; + return 0; +} // identityKinematicsWorkFrame() + +int identityKinematicsToolFrame(const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)joints; + (void)fflags; + // joints are axes, so the tool stays square with the machine + *rot = TOOL_FRAME_SPINDLE; + return 0; +} // identityKinematicsToolFrame() + +//---------------------------------------------------------------------- +// toolFrameSolve() +// +// The inverse of the tool orientation, built on nothing but a module's own +// work and tool frame functions, so that supplying those is enough and no +// module has to hand-derive a formula. +// +// The problem is small: the only joints that can turn the tool are rotary +// ones, there are rarely more than three of them, and the orientation is a +// function of those joints alone. So the routine finds which joints move +// transpose(work) * tool, and solves for them by damped least squares from a +// spread of starting points, keeping the roots that are distinct. +// +// Three things are worth naming because they are what the naive version gets +// wrong. +// +// The damping is adaptive. At a singular pose the Jacobian loses rank, and a +// fixed small damping turns the noise in the near-null direction into a step +// of thousands of degrees. Raising the damping when a step fails and lowering +// it when one succeeds is what keeps those poses solvable at all. +// +// The Jacobian is taken with central differences. A one sided difference has +// an error of the same order as the step, and it appears as a spurious small +// singular value, which is exactly what the rank test must not see. +// +// The joint unit is discovered rather than assumed. Every module in the tree +// takes rotary joints in degrees, but the interface does not say so, and the +// search has to cover exactly one turn. Adding a whole turn and asking +// whether the frame came back settles it, and rescaling into a unit where one +// turn is 2*pi makes the damping and the step limits the same on any module. +//---------------------------------------------------------------------- + +#define TFS_MAX_RES 6 // three for the tool axis, three for tool x +#define TFS_ITERS 60 +#define TFS_FD_STEP 1e-6 // internal radians +#define TFS_MOVED_TOL 1e-9 // frame difference that counts as movement +#define TFS_RANK_TOL 1e-4 // a direction worth less than this is free +#define TFS_SOLVED 1e-18 // sum of squared residuals +#define TFS_STEP_LIMIT 0.4 // internal radians per iteration + +typedef struct { + kinsFrameFunc work; + kinsFrameFunc tool; + int num_joints; + const double *seed; + int nfree; + int free[TOOL_FRAME_MAX_FREE]; + double scale[TOOL_FRAME_MAX_FREE]; // joint units per internal radian + int nres; + double want[TFS_MAX_RES]; + double joint[EMCMOT_MAX_JOINTS]; // scratch, rebuilt on every call +} tfs_ctx; + +// transpose(work) * tool at a joint set, as the columns the request names +static int tfs_frame(tfs_ctx *c, const double *joint, double *axis, double *xdir) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix w, t, m; + + if (c->work(joint, &w, &fflags)) { return -1; } + if (c->tool(joint, &t, &fflags)) { return -1; } + toolFrameInWork(&w, &t, &m); + + axis[0] = m.z.x; axis[1] = m.z.y; axis[2] = m.z.z; + xdir[0] = m.x.x; xdir[1] = m.x.y; xdir[2] = m.x.z; + return 0; +} + +// joint values for a point of the internal search space +static void tfs_joints(tfs_ctx *c, const double *u) +{ + int i; + for (i = 0; i < c->num_joints; i++) { c->joint[i] = c->seed[i]; } + for (i = 0; i < c->nfree; i++) { + c->joint[c->free[i]] = u[i] * c->scale[i]; + } +} + +static int tfs_res(tfs_ctx *c, const double *u, double *r) +{ + double axis[3], xdir[3]; + int i; + + tfs_joints(c, u); + if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } + + for (i = 0; i < 3; i++) { r[i] = axis[i] - c->want[i]; } + if (c->nres > 3) { + for (i = 0; i < 3; i++) { r[3+i] = xdir[i] - c->want[3+i]; } + } + return 0; +} + +static double tfs_norm2(const double *r, int n) +{ + double s = 0; + int i; + for (i = 0; i < n; i++) { s += r[i]*r[i]; } + return s; +} + +static int tfs_jac(tfs_ctx *c, const double *u, double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]) +{ + double up[TOOL_FRAME_MAX_FREE], rp[TFS_MAX_RES], rm[TFS_MAX_RES]; + int i, k; + + for (k = 0; k < c->nfree; k++) { + for (i = 0; i < c->nfree; i++) { up[i] = u[i]; } + up[k] = u[k] + TFS_FD_STEP; + if (tfs_res(c, up, rp)) { return -1; } + up[k] = u[k] - TFS_FD_STEP; + if (tfs_res(c, up, rm)) { return -1; } + for (i = 0; i < c->nres; i++) { + J[i][k] = (rp[i] - rm[i]) / (2*TFS_FD_STEP); + } + } + return 0; +} + +// in place inverse of an n by n matrix by Gauss-Jordan with partial pivoting, +// n being at most TOOL_FRAME_MAX_FREE +static int tfs_inv(double A[TOOL_FRAME_MAX_FREE][TOOL_FRAME_MAX_FREE], int n) +{ + double aug[TOOL_FRAME_MAX_FREE][2*TOOL_FRAME_MAX_FREE]; + int i, j, col, piv; + + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { aug[i][j] = A[i][j]; } + for (j = 0; j < n; j++) { aug[i][n+j] = (i == j) ? 1.0 : 0.0; } + } + for (col = 0; col < n; col++) { + piv = col; + for (i = col+1; i < n; i++) { + if (fabs(aug[i][col]) > fabs(aug[piv][col])) { piv = i; } + } + if (fabs(aug[piv][col]) < 1e-300) { return -1; } + if (piv != col) { + for (j = 0; j < 2*n; j++) { + double sw = aug[col][j]; aug[col][j] = aug[piv][j]; aug[piv][j] = sw; + } + } + { + double d = aug[col][col]; + for (j = 0; j < 2*n; j++) { aug[col][j] /= d; } + } + for (i = 0; i < n; i++) { + double f = aug[i][col]; + if (i == col || f == 0.0) { continue; } + for (j = 0; j < 2*n; j++) { aug[i][j] -= f*aug[col][j]; } + } + } + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { A[i][j] = aug[i][n+j]; } + } + return 0; +} + +// rank by counting pivots, which is all that is needed to say how many +// directions the request leaves free +static int tfs_rank(const double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE], int m, int n) +{ + double a[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]; + double big = 0; + int i, j, col, piv, rank = 0; + + for (i = 0; i < m; i++) { + for (j = 0; j < n; j++) { + a[i][j] = J[i][j]; + if (fabs(a[i][j]) > big) { big = fabs(a[i][j]); } + } + } + if (big <= 0) { return 0; } + + for (col = 0; col < n && rank < m; col++) { + piv = rank; + for (i = rank+1; i < m; i++) { + if (fabs(a[i][col]) > fabs(a[piv][col])) { piv = i; } + } + if (fabs(a[piv][col]) < TFS_RANK_TOL*big) { continue; } + if (piv != rank) { + for (j = 0; j < n; j++) { + double sw = a[rank][j]; a[rank][j] = a[piv][j]; a[piv][j] = sw; + } + } + for (i = rank+1; i < m; i++) { + double f = a[i][col]/a[rank][col]; + for (j = 0; j < n; j++) { a[i][j] -= f*a[rank][j]; } + } + rank++; + } + return rank; +} + +// damped least squares with adaptive damping. Returns 1 when the residual is +// down to the solved threshold, 0 otherwise, and leaves u where it stopped. +static int tfs_levmar(tfs_ctx *c, double *u) +{ + double r[TFS_MAX_RES], r2[TFS_MAX_RES]; + double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]; + double A[TOOL_FRAME_MAX_FREE][TOOL_FRAME_MAX_FREE]; + double g[TOOL_FRAME_MAX_FREE], step[TOOL_FRAME_MAX_FREE]; + double u2[TOOL_FRAME_MAX_FREE]; + double f, f2, lambda = 1e-3; + int i, j, k, it; + + if (tfs_res(c, u, r)) { return 0; } + f = tfs_norm2(r, c->nres); + + for (it = 0; it < TFS_ITERS && f > TFS_SOLVED; it++) { + double trace = 0, big = 0; + + if (tfs_jac(c, u, J)) { return 0; } + + for (i = 0; i < c->nfree; i++) { + for (j = 0; j < c->nfree; j++) { + double s = 0; + for (k = 0; k < c->nres; k++) { s += J[k][i]*J[k][j]; } + A[i][j] = s; + } + trace += A[i][i]; + g[i] = 0; + for (k = 0; k < c->nres; k++) { g[i] += J[k][i]*r[k]; } + } + trace = trace/c->nfree + 1e-30; + + for (i = 0; i < c->nfree; i++) { A[i][i] += lambda*trace; } + if (tfs_inv(A, c->nfree)) { return 0; } + + for (i = 0; i < c->nfree; i++) { + step[i] = 0; + for (j = 0; j < c->nfree; j++) { step[i] -= A[i][j]*g[j]; } + if (fabs(step[i]) > big) { big = fabs(step[i]); } + } + if (big > TFS_STEP_LIMIT) { + for (i = 0; i < c->nfree; i++) { step[i] *= TFS_STEP_LIMIT/big; } + } + for (i = 0; i < c->nfree; i++) { u2[i] = u[i] + step[i]; } + + if (tfs_res(c, u2, r2)) { return 0; } + f2 = tfs_norm2(r2, c->nres); + + if (f2 < f) { + for (i = 0; i < c->nfree; i++) { u[i] = u2[i]; } + for (i = 0; i < c->nres; i++) { r[i] = r2[i]; } + f = f2; + lambda *= 0.3; + if (lambda < 1e-12) { lambda = 1e-12; } + } else { + lambda *= 4.0; + if (lambda > 1e12) { break; } + } + } + return f <= TFS_SOLVED; +} + +static double tfs_wrap(double a) +{ + while (a > PM_PI) { a -= 2*PM_PI; } + while (a < -PM_PI) { a += 2*PM_PI; } + return a; +} + +// which joints turn the tool, and what one turn of each is worth in its own +// units. Returns the count, or -1 if a joint moves the tool without having a +// period, which the search has no way to bound. +static int tfs_survey(tfs_ctx *c) +{ + double base_axis[3], base_x[3], axis[3], xdir[3]; + static const double candidate[2] = { 360.0, 2*PM_PI }; + int i, k, n = 0; + + for (i = 0; i < c->num_joints; i++) { c->joint[i] = c->seed[i]; } + if (tfs_frame(c, c->joint, base_axis, base_x)) { return -1; } + + for (i = 0; i < c->num_joints; i++) { + double moved = 0; + int p; + + for (k = 0; k < c->num_joints; k++) { c->joint[k] = c->seed[k]; } + c->joint[i] = c->seed[i] + 1e-4; + if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } + for (k = 0; k < 3; k++) { + if (fabs(axis[k] - base_axis[k]) > moved) { moved = fabs(axis[k] - base_axis[k]); } + if (fabs(xdir[k] - base_x[k]) > moved) { moved = fabs(xdir[k] - base_x[k]); } + } + if (moved <= TFS_MOVED_TOL) { continue; } + + if (n >= TOOL_FRAME_MAX_FREE) { return -1; } + + c->scale[n] = 0; + for (p = 0; p < 2; p++) { + double back = 0; + c->joint[i] = c->seed[i] + candidate[p]; + if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } + for (k = 0; k < 3; k++) { + if (fabs(axis[k] - base_axis[k]) > back) { back = fabs(axis[k] - base_axis[k]); } + if (fabs(xdir[k] - base_x[k]) > back) { back = fabs(xdir[k] - base_x[k]); } + } + if (back <= TFS_MOVED_TOL) { + c->scale[n] = candidate[p]/(2*PM_PI); + break; + } + } + if (c->scale[n] == 0) { return -1; } + + c->free[n] = i; + n++; + } + c->nfree = n; + return n; +} + +// enumerate the roots for whatever the context currently constrains +static int tfs_search(tfs_ctx *c, + double *solutions, + int max_solutions, + int *free_directions) +{ + double kept[TOOL_FRAME_MAX_SOLUTIONS][TOOL_FRAME_MAX_FREE]; + double u[TOOL_FRAME_MAX_FREE], useed[TOOL_FRAME_MAX_FREE]; + double r[TFS_MAX_RES], J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]; + int index[TOOL_FRAME_MAX_FREE]; + int found = 0, per_axis, first = 1, i, k; + + for (i = 0; i < TOOL_FRAME_MAX_FREE; i++) { u[i] = 0; useed[i] = 0; } + + // nothing on this machine turns the tool, so the only candidate is where + // the machine already is + if (c->nfree == 0) { + if (tfs_res(c, u, r)) { return -1; } + if (tfs_norm2(r, c->nres) > TFS_SOLVED) { return 0; } + for (i = 0; i < c->num_joints; i++) { solutions[i] = c->seed[i]; } + if (free_directions) { free_directions[0] = 0; } + return 1; + } + + for (i = 0; i < c->nfree; i++) { + useed[i] = c->seed[c->free[i]] / c->scale[i]; + index[i] = 0; + } + + // Quarter turns of each free joint, starting from where the machine is so + // that a machine with a free direction reports the answer nearest its + // present pose. Two per turn already enters every basin on the machines + // in the tree, and four is the margin for one that is not: the roots are + // few and widely separated, because they come from the two branches of an + // arc cosine and not from anything finely structured. + per_axis = 4; + + for (;;) { + int solved, rank, dup = 0; + + if (first) { + for (i = 0; i < c->nfree; i++) { u[i] = useed[i]; } + } else { + for (i = 0; i < c->nfree; i++) { + u[i] = -PM_PI + (2*PM_PI*index[i])/per_axis; + } + } + + solved = tfs_levmar(c, u); + if (solved) { + for (i = 0; i < c->nfree; i++) { u[i] = tfs_wrap(u[i]); } + if (tfs_res(c, u, r) || tfs_jac(c, u, J)) { return -1; } + + rank = tfs_rank((const double (*)[TOOL_FRAME_MAX_FREE])J, + c->nres, c->nfree); + tfs_joints(c, u); + + // a rank deficient root means the request does not pin the machine + // down and the answer is a continuum. Report this one point of it + // and say so, rather than returning samples of a curve alongside + // roots that mean something else. + if (c->nfree - rank > 0) { + for (i = 0; i < c->num_joints; i++) { solutions[i] = c->joint[i]; } + if (free_directions) { free_directions[0] = c->nfree - rank; } + return 1; + } + + // Two roots are the same pose if going from one to the other + // does not move the tool. That covers landing on a root already + // found, and it also covers the case a distance test would get + // wrong: near a singularity the search reaches points a long way + // apart in joint values whose frames differ by less than it can + // resolve, and those are one answer and not several. + for (k = 0; k < found; k++) { + double mid[TOOL_FRAME_MAX_FREE] = {0}; + + for (i = 0; i < c->nfree; i++) { + mid[i] = kept[k][i] + tfs_wrap(u[i] - kept[k][i])/2; + } + if (tfs_res(c, mid, r)) { return -1; } + if (tfs_norm2(r, c->nres) <= TFS_SOLVED) { dup = 1; break; } + } + + if (!dup) { + // the dedupe evaluated other points, so rebuild this one + tfs_joints(c, u); + for (i = 0; i < c->num_joints; i++) { + solutions[found*c->num_joints + i] = c->joint[i]; + } + if (free_directions) { free_directions[found] = 0; } + for (i = 0; i < c->nfree; i++) { kept[found][i] = u[i]; } + found++; + if (found >= max_solutions) { return found; } + } + } + + if (first) { first = 0; continue; } + + for (i = 0; i < c->nfree; i++) { + if (++index[i] < per_axis) { break; } + index[i] = 0; + } + if (i == c->nfree) { break; } + } + + return found; +} + +// the turn about the tool axis that carries the tool x this pose achieves onto +// the one the caller asked for +static int tfs_spin(tfs_ctx *c, const double *joint, + const PmCartesian *x_in_work, double *spin) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix w, t, m; + double along_x, along_y; + + if (c->work(joint, &w, &fflags)) { return -1; } + if (c->tool(joint, &t, &fflags)) { return -1; } + toolFrameInWork(&w, &t, &m); + + along_x = m.x.x*x_in_work->x + m.x.y*x_in_work->y + m.x.z*x_in_work->z; + along_y = m.y.x*x_in_work->x + m.y.y*x_in_work->y + m.y.z*x_in_work->z; + + *spin = atan2(along_y, along_x); + return 0; +} + +int toolFrameSolve(kinsFrameFunc work, + kinsFrameFunc tool, + int num_joints, + const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin) +{ + tfs_ctx c; + int found, i; + + if (!work || !tool || !seed || !solutions || !axis_in_work + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS + || max_solutions <= 0) { + return -1; + } + if (max_solutions > TOOL_FRAME_MAX_SOLUTIONS) { + max_solutions = TOOL_FRAME_MAX_SOLUTIONS; + } + + c.work = work; + c.tool = tool; + c.num_joints = num_joints; + c.seed = seed; + c.nres = x_in_work ? 6 : 3; + c.want[0] = axis_in_work->x; + c.want[1] = axis_in_work->y; + c.want[2] = axis_in_work->z; + if (x_in_work) { + double square = axis_in_work->x * x_in_work->x + + axis_in_work->y * x_in_work->y + + axis_in_work->z * x_in_work->z; + + // the two vectors are two axes of one frame, so a request where they + // are not at right angles is not a frame and cannot be reached by + // anything + if (fabs(square) > 1e-6) { return -1; } + + c.want[3] = x_in_work->x; + c.want[4] = x_in_work->y; + c.want[5] = x_in_work->z; + } + + if (tfs_survey(&c) < 0) { return -1; } + + found = tfs_search(&c, solutions, max_solutions, free_directions); + if (found != 0 || !x_in_work) { + if (tool_spin) { + for (i = 0; i < (found > 0 ? found : 0); i++) { tool_spin[i] = 0; } + } + return found; + } + + // The joints cannot place tool x, which is the ordinary case: a five axis + // machine spends both rotaries reaching the tool axis and the turn about + // that axis is not a joint at all. It is still reachable, as a rotation + // of the frame rather than a motion of the machine, so answer with the + // poses that reach the axis and the turn that finishes the job. That is + // what a control does with a Heidenhain base vector or a Fanuc G68.2 + // block, neither of which refuses the program for asking. + if (!tool_spin) { return 0; } + + c.nres = 3; + found = tfs_search(&c, solutions, max_solutions, free_directions); + if (found <= 0) { return found; } + + for (i = 0; i < found; i++) { + if (tfs_spin(&c, solutions + i*num_joints, x_in_work, &tool_spin[i])) { + return -1; + } + } + return found; +} + +//---------------------------------------------------------------------- +// The Jacobian. See kinematics.h for what it is and which way it points. +//---------------------------------------------------------------------- + +static void kj_zero(double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int j, a; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } +} + +// pose coordinate a of p, in EmcPose order +static double *kj_coord(EmcPose *p, int a) +{ + switch (a) { + 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; + } +} + +int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose p; + int j, a; + + if (!inverse || !joint || !world || !jac + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + + kj_zero(jac); + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p = *world; + // the joint array every call sees starts at the machine's own + // position, for a module that reads it before writing it + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&p, a) += KINS_JACOBIAN_STEP; + if (inverse(&p, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&p, a) -= 2 * KINS_JACOBIAN_STEP; + if (inverse(&p, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < num_joints; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsJacobianFromInverse() + +int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int jno, a; + + if (!map_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsJacobianFromMappedAxes before map_initialized\n"); + return -1; + } + if (max_joints <= 0 || max_joints > EMCMOT_MAX_JOINTS) { return -1; } + + kj_zero(jac); + + for (jno = 0; jno < max_joints; jno++) { + int bit = 1< + + 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 + +#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}; +static KT ktools[SWITCHKINS_MAX_TYPES] = {NULL}; +static KT kworks[SWITCHKINS_MAX_TYPES] = {NULL}; +static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL}; +static KJ kjacs[SWITCHKINS_MAX_TYPES] = {NULL}; +static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; + +// 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() + +int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !ktools[switchkins_type]) { + return -1; // this type does not supply one; not an error + } + r = ktools[switchkins_type](joint, rot, fflags); + if (r) { return r; } + + // the type answers in its own frame; put it in the convention here so + // no module has to get the half turn right for itself + return toolFrameApplyNative(rot, &knative[switchkins_type]); +} // kinematicsToolFrame() + +int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !kworks[switchkins_type]) { + return -1; // this type does not supply one; not an error + } + // no native rotation here: the work frame has no tool axis to point the + // wrong way, so there are not two conventions for it to be caught between + return kworks[switchkins_type](joint, rot, fflags); +} // kinematicsWorkFrame() + +int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin) +{ + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !ktools[switchkins_type] + || !kworks[switchkins_type]) { + return -1; // this type does not report its frames, so it cannot answer + } + + // a type that derived the answer by hand knows its own degenerate poses + // and is faster than a search, so it wins where it exists + if (ktinvs[switchkins_type]) { + return ktinvs[switchkins_type](axis_in_work, x_in_work, seed, + solutions, max_solutions, + free_directions, tool_spin); + } + + // the dispatch itself is what the search calls, so the native rotation + // and the per-type lookup are already accounted for + return toolFrameSolve(kinematicsWorkFrame, kinematicsToolFrame, + kp.max_joints, + axis_in_work, x_in_work, seed, + solutions, max_solutions, free_directions, + tool_spin); +} // kinematicsToolFrameInverse() + +int kinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + if (switchkins_type < 0 || switchkins_type >= kins_count) { + return -1; + } + // a closed form is exact and knows its own singular poses + if (kjacs[switchkins_type]) { + return kjacs[switchkins_type](joint, world, jac, iflags); + } + // otherwise the type's own inverse, differenced. The type function + // rather than the dispatch, so this cannot recurse through a switch. + if (!kinvs[switchkins_type]) { return -1; } + return kinsJacobianFromInverse(kinvs[switchkins_type], kp.max_joints, + joint, world, iflags, jac); +} // kinematicsJacobian() + +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() + +int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, + const PmRotationMatrix *native) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterFrames: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + // check the declared rotation once here rather than on every call + if (!native || !toolFrameIsProper(native)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterFrames: switchkins-type %d" + " declared a rotation that is not orthonormal with" + " determinant +1\n", ktype); + register_error = 1; + return -1; + } + kworks[ktype] = kwork; + ktools[ktype] = ktool; + knative[ktype] = *native; + return 0; +} // switchkinsRegisterFrames() + +int switchkinsRegisterJacobian(int ktype, KJ kjac) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterJacobian: BAD switchkins_type" + " <%d> (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + kjacs[ktype] = kjac; + return 0; +} // switchkinsRegisterJacobian() + +int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterToolFrameInverse: BAD" + " switchkins_type <%d> (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + ktinvs[ktype] = kinv; + return 0; +} // switchkinsRegisterToolFrameInverse() + +EXPORT_SYMBOL(kinematicsSwitchable); +EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsToolFrameInverse); +EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(switchkinsRegister); +EXPORT_SYMBOL(switchkinsRegisterFrames); +EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); +EXPORT_SYMBOL(switchkinsRegisterJacobian); +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;} + + // an identity type answers the tool frame the same way whichever module + // asked for it, so supply it here rather than in every switchkinsSetup() + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (!ktools[i] && kfwds[i] == identityKinematicsForward) { + kworks[i] = identityKinematicsWorkFrame; + ktools[i] = identityKinematicsToolFrame; + knative[i] = TOOL_FRAME_SPINDLE; + } + // and its Jacobian is exact, so do not difference for it + if (!kjacs[i] && kfwds[i] == identityKinematicsForward) { + kjacs[i] = identityKinematicsJacobian; + } + } + + // 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 df8dfbafa8c..175bfee28f7 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 @@ -85,4 +85,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 e6814434b72..4a2b287da9d 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 @@ -27,7 +26,7 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// Use EXTRA_SETUP() for pins and params used by kinematics. +// 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; @@ -36,20 +35,10 @@ license "GPL"; author "David Mueller"; ;; -#include +#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; - - //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 @@ -60,111 +49,30 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "millturn" - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) 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; - 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(kinematicsJacobian); -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() -{ - 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,77 +80,68 @@ 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() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int turnKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - int r, c; + int R, C; (void)j; (void)pos; (void)iflags; - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { - for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } - } - // the derivative of kinematicsInverse() for each type: which joint - // follows which pose coordinate, and in which sense - switch (switchkins_type) { - case 0: - jac[0][0] = 1; - jac[1][1] = 1; - jac[2][2] = 1; - jac[3][3] = 1; - break; - case 1: - jac[2][0] = 1; - jac[1][1] = -1; - jac[0][2] = 1; - jac[3][3] = 1; - break; + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } + // the derivative of turnKinematicsInverse(): which joint follows which + // pose coordinate, and in which sense + jac[2][0] = 1; + jac[1][1] = -1; + jac[0][2] = 1; + jac[3][3] = 1; return 0; -} // kinematicsJacobian() +} // turnKinematicsJacobian() + +// 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; } + if (switchkinsRegisterJacobian(1, turnKinematicsJacobian)) { 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 a040e757e12..c1ef4b1a121 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 @@ -31,6 +30,7 @@ chapter (docs/src/motion/switchkins.txt) """; pin out si32 dummy=0"one pin needed to satisfy halcompile requirement"; + option extra_setup; license "GPL"; @@ -38,117 +38,61 @@ author "David Mueller"; ;; #include -#include -static struct haldata { +#include - // Declare hal pin pointers used for xyzab_tdr kinematics: +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +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; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzab_tdr_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) 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; - 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(kinematicsJacobian); -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() -{ - 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 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; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + 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,53 +150,38 @@ 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() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tdrKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (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 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 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(tdrdata->x_offset); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); double sa = sin(pos->a*TO_RAD); double ca = cos(pos->a*TO_RAD); double sb = sin(pos->b*TO_RAD); @@ -277,43 +189,58 @@ int kinematicsJacobian(const double *j, 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; - int r, c; + int R, C; - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { - for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - switch (switchkins_type) { - case 0: // ====================== IDENTITY kinematics JACOBIAN ==================== - jac[0][0] = 1; - jac[1][1] = 1; - jac[2][2] = 1; - jac[3][3] = 1; - jac[4][4] = 1; - break; - case 1: // ========================= TCP kinematics JACOBIAN ====================== - // the TCP inverse above differentiated: its coefficients of - // qx, qy and qz for the linear columns, and the same terms - // with a or b advanced a quarter turn for the rotary columns - jac[0][0] = cb; - jac[0][1] = sa*sb; - jac[0][2] = -ca*sb; - jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; - jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; - - jac[1][1] = ca; - jac[1][2] = sa; - jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; - - jac[2][0] = sb; - jac[2][1] = -sa*cb; - jac[2][2] = ca*cb; - jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; - jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; - - jac[3][3] = 1; - jac[4][4] = 1; - break; - } + // tdrKinematicsInverse() differentiated: its coefficients of qx, qy + // and qz for the linear columns, and the same terms with a or b + // advanced a quarter turn for the rotary columns + jac[0][0] = cb; + jac[0][1] = sa*sb; + jac[0][2] = -ca*sb; + jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; + jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; + + jac[1][1] = ca; + jac[1][2] = sa; + jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; + + jac[2][0] = sb; + jac[2][1] = -sa*cb; + jac[2][2] = ca*cb; + jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; + jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; return 0; -} // kinematicsJacobian() +} // tdrKinematicsJacobian() + +// 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; } + if (switchkinsRegisterJacobian(1, tdrKinematicsJacobian)) { 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 efe25aa1e46..7a2b48d6659 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -4,6 +4,11 @@ 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; @@ -14,8 +19,11 @@ 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 @@ -36,123 +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; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzacb_trsrn_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) 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; - 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(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - - + 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; -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) { - 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 @@ -197,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); @@ -253,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); @@ -293,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; @@ -304,98 +220,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -// These modules do not link kins_util.c, so they cannot reach the shared -// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. -static void frame_square_with_machine(PmRotationMatrix *rot) -{ - rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; - rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; - rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; -} - -int kinematicsToolFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Sv = sin(nu*TO_RAD); - double Cv = cos(nu*TO_RAD); - double Ss = sin(j[4]*TO_RAD); - double Cs = cos(j[4]*TO_RAD); - double Sp = sin(j[5]*TO_RAD); - double Cp = cos(j[5]*TO_RAD); - double r = Cs + Sv*Sv*(1-Cs); - double s = Cs + Cv*Cv*(1-Cs); - double t = Sv*Cv*(1-Cs); - int a, b, k; - - // identity kinematics, and tool kinematics where the world axes are the - // tool axes by construction, both leave the tool square with the machine - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the primary joint turns the head about z - const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; - - // the nutating secondary joint - const double Rs[3][3] = {{Cs, -Cv*Ss, Sv*Ss}, - {Cv*Ss, r, t}, - {-Sv*Ss, t, s}}; - - double M[3][3]; - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - M[a][b] = 0; - for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } - } - } - - rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; - rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; - rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; - - return 0; -} // kinematicsToolFrame() + (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double Sw = sin(j[3]*TO_RAD); - double Cw = cos(j[3]*TO_RAD); - - // in tool kinematics the world axes are the tool axes, so the work is not - // being reported against the machine and there is nothing to turn - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the A joint carries the work: its frame in machine coordinates - // is a rotation about x by the joint value - const double W[3][3] = {{1, 0, 0}, {0, Cw, Sw}, {0, -Sw, Cw}}; - - rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; - rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; - rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; - - return 0; -} // kinematicsWorkFrame() - -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ (void)iflags; - (void)fflags; + 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 @@ -443,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); @@ -496,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); @@ -540,38 +370,112 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; + } + + return 0; +} // 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() + +// The head answers in the convention already, so the native rotation +// registered with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = hal_get_real(haldata->nut_angle); // degrees + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Ss = sin(j[4]*TO_RAD); + double Cs = cos(j[4]*TO_RAD); + double Sp = sin(j[5]*TO_RAD); + double Cp = cos(j[5]*TO_RAD); + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int a, b, k; + + // the primary joint turns the head about z + const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; + + // the nutating secondary joint + const double Rs[3][3] = {{Cs, -Cv*Ss, Sv*Ss}, + {Cv*Ss, r, t}, + {-Sv*Ss, t, s}}; - break; + double M[3][3]; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + M[a][b] = 0; + for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } + } } + rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; + rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; + rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; + + return 0; +} // tcpKinematicsToolFrame() + +static int tcpKinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double Sw = sin(j[3]*TO_RAD); + double Cw = cos(j[3]*TO_RAD); + + // the A joint carries the work: its frame in machine coordinates + // is a rotation about x by the joint value + const double W[3][3] = {{1, 0, 0}, {0, Cw, Sw}, {0, -Sw, Cw}}; + + rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; + rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; + rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; + return 0; -} // kinematicsInverse() +} // tcpKinematicsWorkFrame() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tcpKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - // the same geometry as kinematicsInverse(), read the same way + // the same geometry as trsrnInverse(), read the same way double Ly = hal_get_real(haldata->y_pivot); double Lz = hal_get_real(haldata->z_pivot); double Dx = hal_get_real(haldata->x_offset); double Dy = hal_get_real(haldata->y_offset); double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees double Dt = hal_get_real(haldata->tool_offset_z); double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); - double Stc = sin(tc); - double Ctc = cos(tc); // The TCP inverse reads the rotary angles from its joint argument, // where the machine is, and its own pose words for the same angles @@ -579,105 +483,149 @@ int kinematicsJacobian(const double *j, // against the pose, which is what a consumer multiplies by. double Sw = sin(pos->a*TO_RAD); double Cw = cos(pos->a*TO_RAD); - double Ss = 0, Cs = 0, Sp = 0, Cp = 0; - double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + double Ss = sin(pos->b*TO_RAD); + double Cs = cos(pos->b*TO_RAD); + double Sp = sin(pos->c*TO_RAD); + double Cp = cos(pos->c*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double t = Sv*Cv*(1-Cs); // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, // SvSs) and the primary angle (Sp, Cp), per degree - double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; - double dSp = 0, dCp = 0; + double dSs = Cs*TO_RAD; + double dr = -Ss*Cv*Cv*TO_RAD; + double ds = -Ss*Sv*Sv*TO_RAD; + double dt_ = Sv*Cv*Ss*TO_RAD; + double dCvSs = Cv*dSs; + double dSvSs = Sv*dSs; + double dSp = Cp*TO_RAD; + double dCp = -Sp*TO_RAD; double Qy = pos->tran.y; double Qz = pos->tran.z; - double Ay, Az; // the two lever arms the table turns about + // the two lever arms the table turns about + double Ay = Dray + Dy + Ly - Qy; + double Az = Draz + Dt + Lz - Qz; int R, C; for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - switch (switchkins_type) { + // j[0]: Qx plus terms in the head angles only + jac[0][0] = 1; + jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; + jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx + - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; + + // j[1]: -Cw*Ay - Az*Sw plus head terms + jac[1][1] = Cw; + jac[1][2] = Sw; + jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; + jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; + jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Ly; + + // j[2]: -Cw*Az + Ay*Sw plus head terms + jac[2][1] = -Sw; + jac[2][2] = Cw; + jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; + jac[2][4] = (Dt + Lz)*ds + Ly*dt_; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // tcpKinematicsJacobian() - case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== - for (R = 0; R < 6; R++) { jac[R][R] = 1; } - break; +static int toolKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)pos; + (void)iflags; - case 1: // ========================= TCP kinematics JACOBIAN - Ss = sin(pos->b*TO_RAD); - Cs = cos(pos->b*TO_RAD); - Sp = sin(pos->c*TO_RAD); - Cp = cos(pos->c*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees - dSs = Cs*TO_RAD; - dr = -Ss*Cv*Cv*TO_RAD; - ds = -Ss*Sv*Sv*TO_RAD; - dt_ = Sv*Cv*Ss*TO_RAD; - dCvSs = Cv*dSs; - dSvSs = Sv*dSs; - dSp = Cp*TO_RAD; - dCp = -Sp*TO_RAD; - - Ay = Dray + Dy + Ly - Qy; - Az = Draz + Dt + Lz - Qz; - - // j[0]: Qx plus terms in the head angles only - jac[0][0] = 1; - jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; - jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx - - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; - - // j[1]: -Cw*Ay - Az*Sw plus head terms - jac[1][1] = Cw; - jac[1][2] = Sw; - jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; - jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; - jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) - - (CvSs*dSp - dCp*r)*Ly; - - // j[2]: -Cw*Az + Ay*Sw plus head terms - jac[2][1] = -Sw; - jac[2][2] = Cw; - jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; - jac[2][4] = (Dt + Lz)*ds + Ly*dt_; - - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - - case 2: // ========================= TOOL kinematics JACOBIAN - // the head angles come from pins, so the inverse is linear in - // the pose and the rows are its coefficients - Ss = sin(theta_2*TO_RAD); - Cs = cos(theta_2*TO_RAD); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + double Ss = sin(theta_2*TO_RAD); + double Cs = cos(theta_2*TO_RAD); + double Sp = sin(theta_1*TO_RAD); + double Cp = cos(theta_1*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } - jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); - jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); - jac[0][2] = (Cp*SvSs - Sp*t); + jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[0][2] = (Cp*SvSs - Sp*t); - jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); - jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); - jac[1][2] = (Sp*SvSs + Cp*t); + jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[1][2] = (Sp*SvSs + Cp*t); - jac[2][0] = -(Ctc*SvSs - Stc*t); - jac[2][1] = (Stc*SvSs + Ctc*t); - jac[2][2] = s; + jac[2][0] = -(Ctc*SvSs - Stc*t); + jac[2][1] = (Stc*SvSs + Ctc*t); + jac[2][2] = s; - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - } + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; return 0; -} // kinematicsJacobian() +} // toolKinematicsJacobian() + +// 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; } + if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, + tcpKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } + // the tool kinematics report in tool axes, so the tool is square with + // the world by construction and nothing turns the work against it + if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, + identityKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { 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 f844a422953..ee535daaa6e 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -4,6 +4,11 @@ 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; @@ -14,8 +19,11 @@ 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 @@ -36,125 +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; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#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; - 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(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - - + 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; -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) { - 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 @@ -200,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); @@ -260,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); @@ -300,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; @@ -311,98 +225,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -// These modules do not link kins_util.c, so they cannot reach the shared -// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. -static void frame_square_with_machine(PmRotationMatrix *rot) -{ - rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; - rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; - rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; -} - -int kinematicsToolFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Sv = sin(nu*TO_RAD); - double Cv = cos(nu*TO_RAD); - double Ss = sin(j[3]*TO_RAD); - double Cs = cos(j[3]*TO_RAD); - double Sp = sin(j[5]*TO_RAD); - double Cp = cos(j[5]*TO_RAD); - double r = Cs + Sv*Sv*(1-Cs); - double s = Cs + Cv*Cv*(1-Cs); - double t = Sv*Cv*(1-Cs); - int a, b, k; - - // identity kinematics, and tool kinematics where the world axes are the - // tool axes by construction, both leave the tool square with the machine - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the primary joint turns the head about z - const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; - - // the nutating secondary joint - const double Rs[3][3] = {{r, -Cv*Ss, t}, - {Cv*Ss, Cs, -Sv*Ss}, - {t, Sv*Ss, s}}; - - double M[3][3]; - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - M[a][b] = 0; - for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } - } - } - - rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; - rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; - rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; - - return 0; -} // kinematicsToolFrame() + (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double Sw = sin(j[4]*TO_RAD); - double Cw = cos(j[4]*TO_RAD); - - // in tool kinematics the world axes are the tool axes, so the work is not - // being reported against the machine and there is nothing to turn - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the B joint carries the work: its frame in machine coordinates - // is a rotation about y by the joint value - const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}}; - - rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; - rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; - rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; - - return 0; -} // kinematicsWorkFrame() - -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ (void)iflags; - (void)fflags; + 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 @@ -448,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); @@ -501,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); @@ -545,38 +373,112 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; + } + + return 0; +} // 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() + +// The head answers in the convention already, so the native rotation +// registered with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = hal_get_real(haldata->nut_angle); // degrees + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Ss = sin(j[3]*TO_RAD); + double Cs = cos(j[3]*TO_RAD); + double Sp = sin(j[5]*TO_RAD); + double Cp = cos(j[5]*TO_RAD); + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int a, b, k; + + // the primary joint turns the head about z + const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; + + // the nutating secondary joint + const double Rs[3][3] = {{r, -Cv*Ss, t}, + {Cv*Ss, Cs, -Sv*Ss}, + {t, Sv*Ss, s}}; - break; + double M[3][3]; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + M[a][b] = 0; + for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } + } } + rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; + rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; + rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; + + return 0; +} // tcpKinematicsToolFrame() + +static int tcpKinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double Sw = sin(j[4]*TO_RAD); + double Cw = cos(j[4]*TO_RAD); + + // the B joint carries the work: its frame in machine coordinates + // is a rotation about y by the joint value + const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}}; + + rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; + rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; + rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; + return 0; -} // kinematicsInverse() +} // tcpKinematicsWorkFrame() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tcpKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - // the same geometry as kinematicsInverse(), read the same way + // the same geometry as trsrnInverse(), read the same way double Lx = hal_get_real(haldata->x_pivot); double Lz = hal_get_real(haldata->z_pivot); double Dx = hal_get_real(haldata->x_offset); double Dy = hal_get_real(haldata->y_offset); double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees double Dt = hal_get_real(haldata->tool_offset_z); double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); - double Stc = sin(tc); - double Ctc = cos(tc); // The TCP inverse reads the rotary angles from its joint argument, // where the machine is, and its own pose words for the same angles @@ -584,105 +486,149 @@ int kinematicsJacobian(const double *j, // against the pose, which is what a consumer multiplies by. double Sw = sin(pos->b*TO_RAD); double Cw = cos(pos->b*TO_RAD); - double Ss = 0, Cs = 0, Sp = 0, Cp = 0; - double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + double Ss = sin(pos->a*TO_RAD); + double Cs = cos(pos->a*TO_RAD); + double Sp = sin(pos->c*TO_RAD); + double Cp = cos(pos->c*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double t = Sv*Cv*(1-Cs); // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, // SvSs) and the primary angle (Sp, Cp), per degree - double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; - double dSp = 0, dCp = 0; + double dSs = Cs*TO_RAD; + double dr = -Ss*Cv*Cv*TO_RAD; + double ds = -Ss*Sv*Sv*TO_RAD; + double dt_ = Sv*Cv*Ss*TO_RAD; + double dCvSs = Cv*dSs; + double dSvSs = Sv*dSs; + double dSp = Cp*TO_RAD; + double dCp = -Sp*TO_RAD; double Qx = pos->tran.x; double Qz = pos->tran.z; - double Ax, Az; // the two lever arms the table turns about + // the two lever arms the table turns about + double Ax = Drax + Dx + Lx - Qx; + double Az = Draz + Dt + Lz - Qz; int R, C; for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - switch (switchkins_type) { + // j[0]: -Cw*Ax + Az*Sw plus head terms + jac[0][0] = Cw; + jac[0][2] = -Sw; + jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; + jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; + jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Lx; + + // j[1]: Qy plus head terms + jac[1][1] = 1; + jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; + jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy + + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; + + // j[2]: -Cw*Az - Ax*Sw plus head terms + jac[2][0] = Sw; + jac[2][2] = Cw; + jac[2][3] = (Dt + Lz)*ds + Lx*dt_; + jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // tcpKinematicsJacobian() - case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== - for (R = 0; R < 6; R++) { jac[R][R] = 1; } - break; +static int toolKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)pos; + (void)iflags; - case 1: // ========================= TCP kinematics JACOBIAN - Ss = sin(pos->a*TO_RAD); - Cs = cos(pos->a*TO_RAD); - Sp = sin(pos->c*TO_RAD); - Cp = cos(pos->c*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees - dSs = Cs*TO_RAD; - dr = -Ss*Cv*Cv*TO_RAD; - ds = -Ss*Sv*Sv*TO_RAD; - dt_ = Sv*Cv*Ss*TO_RAD; - dCvSs = Cv*dSs; - dSvSs = Sv*dSs; - dSp = Cp*TO_RAD; - dCp = -Sp*TO_RAD; - - Ax = Drax + Dx + Lx - Qx; - Az = Draz + Dt + Lz - Qz; - - // j[0]: -Cw*Ax + Az*Sw plus head terms - jac[0][0] = Cw; - jac[0][2] = -Sw; - jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; - jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; - jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) - - (CvSs*dSp - dCp*r)*Lx; - - // j[1]: Qy plus head terms - jac[1][1] = 1; - jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; - jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy - + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; - - // j[2]: -Cw*Az - Ax*Sw plus head terms - jac[2][0] = Sw; - jac[2][2] = Cw; - jac[2][3] = (Dt + Lz)*ds + Lx*dt_; - jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; - - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - - case 2: // ========================= TOOL kinematics JACOBIAN - // the head angles come from pins, so the inverse is linear in - // the pose and the rows are its coefficients - Ss = sin(theta_2*TO_RAD); - Cs = cos(theta_2*TO_RAD); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + double Ss = sin(theta_2*TO_RAD); + double Cs = cos(theta_2*TO_RAD); + double Sp = sin(theta_1*TO_RAD); + double Cp = cos(theta_1*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } - jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); - jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); - jac[0][2] = (Sp*SvSs + Cp*t); + jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[0][2] = (Sp*SvSs + Cp*t); - jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); - jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); - jac[1][2] = -(Cp*SvSs - Sp*t); + jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[1][2] = -(Cp*SvSs - Sp*t); - jac[2][0] = (Stc*SvSs + Ctc*t); - jac[2][1] = (Ctc*SvSs - Stc*t); - jac[2][2] = s; + jac[2][0] = (Stc*SvSs + Ctc*t); + jac[2][1] = (Ctc*SvSs - Stc*t); + jac[2][2] = s; - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - } + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; return 0; -} // kinematicsJacobian() +} // toolKinematicsJacobian() + +// 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; } + if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, + tcpKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } + // the tool kinematics report in tool axes, so the tool is square with + // the world by construction and nothing turns the work against it + if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, + identityKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() From 531a6f1949e65b52a48c357c9b78c209696f8ce6 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 37/58] 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 e613c18749c..83c24b9077a 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 60c918e19189c410b26552eca49873c76791deb3 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 38/58] 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 027d4a8205a..946db9ba41d 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 a9210b6d917..9ef74299336 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 22374507be2..acc8bf89ebc 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 2155263c55b..5cac4f46318 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 57f36ea9c14..d12e57fbfcb 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 ebe45c46412..526c298f399 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 f475a16d95372f7dff3f62709e40d0115fc70b39 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 39/58] 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 946db9ba41d..cf590d982a8 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 9ef74299336..848f88fde30 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 8c209413b28..bdd37694030 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 acc8bf89ebc..5e381700a88 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 5cac4f46318..ea86e3170ce 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 526c298f399..b010eded5a7 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 58c92a8a1df..666d0ba128f 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 68518d91537..9ac7ed9e3c0 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 5b51e0676dd90de8941c56a96047ab128cd0d4ee 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 40/58] 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 +- share/linuxcnc/kins_util.c | 1145 ------------------------ share/linuxcnc/switchkins.c | 502 ----------- src/Makefile | 1 + src/Makefile.modinc.in | 4 +- src/emc/kinematics/Submakefile | 13 + src/hal/components/switchkinscomp.comp | 34 +- 9 files changed, 40 insertions(+), 1680 deletions(-) delete mode 100644 share/linuxcnc/kins_util.c delete mode 100644 share/linuxcnc/switchkins.c 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/share/linuxcnc/kins_util.c b/share/linuxcnc/kins_util.c deleted file mode 100644 index 392cd046c46..00000000000 --- a/share/linuxcnc/kins_util.c +++ /dev/null @@ -1,1145 +0,0 @@ -/* 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 -#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() - -const PmRotationMatrix TOOL_FRAME_SPINDLE = { - { 1, 0, 0}, // tool x - { 0, 1, 0}, // tool y - { 0, 0, 1} // tool axis -}; - -// half turn about tool x: reverses the tool axis and tool y, keeps tool x, -// and keeps the frame right-handed. Negating the tool axis on its own would -// leave a reflection, which is not a frame any machine can hold. -const PmRotationMatrix TOOL_FRAME_FLANGE = { - { 1, 0, 0}, - { 0, -1, 0}, - { 0, 0, -1} -}; - -int toolFrameIsProper(const PmRotationMatrix *m) -{ - const double c[3][3] = { - { m->x.x, m->y.x, m->z.x }, - { m->x.y, m->y.y, m->z.y }, - { m->x.z, m->y.z, m->z.z } - }; - double det; - int a, b, k; - - for (a = 0; a < 3; a++) { - for (b = a; b < 3; b++) { - double dot = 0; - for (k = 0; k < 3; k++) { dot += c[k][a] * c[k][b]; } - if (fabs(dot - (a == b ? 1.0 : 0.0)) > 1e-9) { return 0; } - } - } - - det = c[0][0] * (c[1][1]*c[2][2] - c[1][2]*c[2][1]) - - c[0][1] * (c[1][0]*c[2][2] - c[1][2]*c[2][0]) - + c[0][2] * (c[1][0]*c[2][1] - c[1][1]*c[2][0]); - - return fabs(det - 1.0) <= 1e-9; -} // toolFrameIsProper() - -int toolFrameApplyNative(PmRotationMatrix *rot, - const PmRotationMatrix *native) -{ - // rot holds the module's own frame, native the rotation relating it to - // the convention, so the answer is rot * native: the declared rotation is - // expressed in the module's frame, not in machine coordinates. - const double r[3][3] = { - { rot->x.x, rot->y.x, rot->z.x }, - { rot->x.y, rot->y.y, rot->z.y }, - { rot->x.z, rot->y.z, rot->z.z } - }; - const double n[3][3] = { - { native->x.x, native->y.x, native->z.x }, - { native->x.y, native->y.y, native->z.y }, - { native->x.z, native->y.z, native->z.z } - }; - double m[3][3]; - int a, b, k; - - if (!toolFrameIsProper(native)) { - rtapi_print_msg(RTAPI_MSG_ERR, - "toolFrameApplyNative: declared rotation is not a proper rotation\n"); - return -1; - } - - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - m[a][b] = 0; - for (k = 0; k < 3; k++) { m[a][b] += r[a][k] * n[k][b]; } - } - } - - rot->x.x = m[0][0]; rot->y.x = m[0][1]; rot->z.x = m[0][2]; - rot->x.y = m[1][0]; rot->y.y = m[1][1]; rot->z.y = m[1][2]; - rot->x.z = m[2][0]; rot->y.z = m[2][1]; rot->z.z = m[2][2]; - - return 0; -} // toolFrameApplyNative() - -int toolFrameInWork(const PmRotationMatrix *work, - const PmRotationMatrix *tool, - PmRotationMatrix *out) -{ - // transpose(work) * tool: both are given against the machine, and - // transposing the work frame turns "machine to work" out of "work to - // machine" without a general inverse, because a rotation is orthonormal - const double w[3][3] = { - { work->x.x, work->y.x, work->z.x }, - { work->x.y, work->y.y, work->z.y }, - { work->x.z, work->y.z, work->z.z } - }; - const double t[3][3] = { - { tool->x.x, tool->y.x, tool->z.x }, - { tool->x.y, tool->y.y, tool->z.y }, - { tool->x.z, tool->y.z, tool->z.z } - }; - double m[3][3]; - int a, b, k; - - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - m[a][b] = 0; - for (k = 0; k < 3; k++) { m[a][b] += w[k][a] * t[k][b]; } - } - } - - out->x.x = m[0][0]; out->y.x = m[0][1]; out->z.x = m[0][2]; - out->x.y = m[1][0]; out->y.y = m[1][1]; out->z.y = m[1][2]; - out->x.z = m[2][0]; out->y.z = m[2][1]; out->z.z = m[2][2]; - - return 0; -} // toolFrameInWork() - -int identityKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // nothing carries the work, so it stays square with the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // identityKinematicsWorkFrame() - -int identityKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // joints are axes, so the tool stays square with the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // identityKinematicsToolFrame() - -//---------------------------------------------------------------------- -// toolFrameSolve() -// -// The inverse of the tool orientation, built on nothing but a module's own -// work and tool frame functions, so that supplying those is enough and no -// module has to hand-derive a formula. -// -// The problem is small: the only joints that can turn the tool are rotary -// ones, there are rarely more than three of them, and the orientation is a -// function of those joints alone. So the routine finds which joints move -// transpose(work) * tool, and solves for them by damped least squares from a -// spread of starting points, keeping the roots that are distinct. -// -// Three things are worth naming because they are what the naive version gets -// wrong. -// -// The damping is adaptive. At a singular pose the Jacobian loses rank, and a -// fixed small damping turns the noise in the near-null direction into a step -// of thousands of degrees. Raising the damping when a step fails and lowering -// it when one succeeds is what keeps those poses solvable at all. -// -// The Jacobian is taken with central differences. A one sided difference has -// an error of the same order as the step, and it appears as a spurious small -// singular value, which is exactly what the rank test must not see. -// -// The joint unit is discovered rather than assumed. Every module in the tree -// takes rotary joints in degrees, but the interface does not say so, and the -// search has to cover exactly one turn. Adding a whole turn and asking -// whether the frame came back settles it, and rescaling into a unit where one -// turn is 2*pi makes the damping and the step limits the same on any module. -//---------------------------------------------------------------------- - -#define TFS_MAX_RES 6 // three for the tool axis, three for tool x -#define TFS_ITERS 60 -#define TFS_FD_STEP 1e-6 // internal radians -#define TFS_MOVED_TOL 1e-9 // frame difference that counts as movement -#define TFS_RANK_TOL 1e-4 // a direction worth less than this is free -#define TFS_SOLVED 1e-18 // sum of squared residuals -#define TFS_STEP_LIMIT 0.4 // internal radians per iteration - -typedef struct { - kinsFrameFunc work; - kinsFrameFunc tool; - int num_joints; - const double *seed; - int nfree; - int free[TOOL_FRAME_MAX_FREE]; - double scale[TOOL_FRAME_MAX_FREE]; // joint units per internal radian - int nres; - double want[TFS_MAX_RES]; - double joint[EMCMOT_MAX_JOINTS]; // scratch, rebuilt on every call -} tfs_ctx; - -// transpose(work) * tool at a joint set, as the columns the request names -static int tfs_frame(tfs_ctx *c, const double *joint, double *axis, double *xdir) -{ - KINEMATICS_FORWARD_FLAGS fflags = 0; - PmRotationMatrix w, t, m; - - if (c->work(joint, &w, &fflags)) { return -1; } - if (c->tool(joint, &t, &fflags)) { return -1; } - toolFrameInWork(&w, &t, &m); - - axis[0] = m.z.x; axis[1] = m.z.y; axis[2] = m.z.z; - xdir[0] = m.x.x; xdir[1] = m.x.y; xdir[2] = m.x.z; - return 0; -} - -// joint values for a point of the internal search space -static void tfs_joints(tfs_ctx *c, const double *u) -{ - int i; - for (i = 0; i < c->num_joints; i++) { c->joint[i] = c->seed[i]; } - for (i = 0; i < c->nfree; i++) { - c->joint[c->free[i]] = u[i] * c->scale[i]; - } -} - -static int tfs_res(tfs_ctx *c, const double *u, double *r) -{ - double axis[3], xdir[3]; - int i; - - tfs_joints(c, u); - if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } - - for (i = 0; i < 3; i++) { r[i] = axis[i] - c->want[i]; } - if (c->nres > 3) { - for (i = 0; i < 3; i++) { r[3+i] = xdir[i] - c->want[3+i]; } - } - return 0; -} - -static double tfs_norm2(const double *r, int n) -{ - double s = 0; - int i; - for (i = 0; i < n; i++) { s += r[i]*r[i]; } - return s; -} - -static int tfs_jac(tfs_ctx *c, const double *u, double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]) -{ - double up[TOOL_FRAME_MAX_FREE], rp[TFS_MAX_RES], rm[TFS_MAX_RES]; - int i, k; - - for (k = 0; k < c->nfree; k++) { - for (i = 0; i < c->nfree; i++) { up[i] = u[i]; } - up[k] = u[k] + TFS_FD_STEP; - if (tfs_res(c, up, rp)) { return -1; } - up[k] = u[k] - TFS_FD_STEP; - if (tfs_res(c, up, rm)) { return -1; } - for (i = 0; i < c->nres; i++) { - J[i][k] = (rp[i] - rm[i]) / (2*TFS_FD_STEP); - } - } - return 0; -} - -// in place inverse of an n by n matrix by Gauss-Jordan with partial pivoting, -// n being at most TOOL_FRAME_MAX_FREE -static int tfs_inv(double A[TOOL_FRAME_MAX_FREE][TOOL_FRAME_MAX_FREE], int n) -{ - double aug[TOOL_FRAME_MAX_FREE][2*TOOL_FRAME_MAX_FREE]; - int i, j, col, piv; - - for (i = 0; i < n; i++) { - for (j = 0; j < n; j++) { aug[i][j] = A[i][j]; } - for (j = 0; j < n; j++) { aug[i][n+j] = (i == j) ? 1.0 : 0.0; } - } - for (col = 0; col < n; col++) { - piv = col; - for (i = col+1; i < n; i++) { - if (fabs(aug[i][col]) > fabs(aug[piv][col])) { piv = i; } - } - if (fabs(aug[piv][col]) < 1e-300) { return -1; } - if (piv != col) { - for (j = 0; j < 2*n; j++) { - double sw = aug[col][j]; aug[col][j] = aug[piv][j]; aug[piv][j] = sw; - } - } - { - double d = aug[col][col]; - for (j = 0; j < 2*n; j++) { aug[col][j] /= d; } - } - for (i = 0; i < n; i++) { - double f = aug[i][col]; - if (i == col || f == 0.0) { continue; } - for (j = 0; j < 2*n; j++) { aug[i][j] -= f*aug[col][j]; } - } - } - for (i = 0; i < n; i++) { - for (j = 0; j < n; j++) { A[i][j] = aug[i][n+j]; } - } - return 0; -} - -// rank by counting pivots, which is all that is needed to say how many -// directions the request leaves free -static int tfs_rank(const double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE], int m, int n) -{ - double a[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]; - double big = 0; - int i, j, col, piv, rank = 0; - - for (i = 0; i < m; i++) { - for (j = 0; j < n; j++) { - a[i][j] = J[i][j]; - if (fabs(a[i][j]) > big) { big = fabs(a[i][j]); } - } - } - if (big <= 0) { return 0; } - - for (col = 0; col < n && rank < m; col++) { - piv = rank; - for (i = rank+1; i < m; i++) { - if (fabs(a[i][col]) > fabs(a[piv][col])) { piv = i; } - } - if (fabs(a[piv][col]) < TFS_RANK_TOL*big) { continue; } - if (piv != rank) { - for (j = 0; j < n; j++) { - double sw = a[rank][j]; a[rank][j] = a[piv][j]; a[piv][j] = sw; - } - } - for (i = rank+1; i < m; i++) { - double f = a[i][col]/a[rank][col]; - for (j = 0; j < n; j++) { a[i][j] -= f*a[rank][j]; } - } - rank++; - } - return rank; -} - -// damped least squares with adaptive damping. Returns 1 when the residual is -// down to the solved threshold, 0 otherwise, and leaves u where it stopped. -static int tfs_levmar(tfs_ctx *c, double *u) -{ - double r[TFS_MAX_RES], r2[TFS_MAX_RES]; - double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]; - double A[TOOL_FRAME_MAX_FREE][TOOL_FRAME_MAX_FREE]; - double g[TOOL_FRAME_MAX_FREE], step[TOOL_FRAME_MAX_FREE]; - double u2[TOOL_FRAME_MAX_FREE]; - double f, f2, lambda = 1e-3; - int i, j, k, it; - - if (tfs_res(c, u, r)) { return 0; } - f = tfs_norm2(r, c->nres); - - for (it = 0; it < TFS_ITERS && f > TFS_SOLVED; it++) { - double trace = 0, big = 0; - - if (tfs_jac(c, u, J)) { return 0; } - - for (i = 0; i < c->nfree; i++) { - for (j = 0; j < c->nfree; j++) { - double s = 0; - for (k = 0; k < c->nres; k++) { s += J[k][i]*J[k][j]; } - A[i][j] = s; - } - trace += A[i][i]; - g[i] = 0; - for (k = 0; k < c->nres; k++) { g[i] += J[k][i]*r[k]; } - } - trace = trace/c->nfree + 1e-30; - - for (i = 0; i < c->nfree; i++) { A[i][i] += lambda*trace; } - if (tfs_inv(A, c->nfree)) { return 0; } - - for (i = 0; i < c->nfree; i++) { - step[i] = 0; - for (j = 0; j < c->nfree; j++) { step[i] -= A[i][j]*g[j]; } - if (fabs(step[i]) > big) { big = fabs(step[i]); } - } - if (big > TFS_STEP_LIMIT) { - for (i = 0; i < c->nfree; i++) { step[i] *= TFS_STEP_LIMIT/big; } - } - for (i = 0; i < c->nfree; i++) { u2[i] = u[i] + step[i]; } - - if (tfs_res(c, u2, r2)) { return 0; } - f2 = tfs_norm2(r2, c->nres); - - if (f2 < f) { - for (i = 0; i < c->nfree; i++) { u[i] = u2[i]; } - for (i = 0; i < c->nres; i++) { r[i] = r2[i]; } - f = f2; - lambda *= 0.3; - if (lambda < 1e-12) { lambda = 1e-12; } - } else { - lambda *= 4.0; - if (lambda > 1e12) { break; } - } - } - return f <= TFS_SOLVED; -} - -static double tfs_wrap(double a) -{ - while (a > PM_PI) { a -= 2*PM_PI; } - while (a < -PM_PI) { a += 2*PM_PI; } - return a; -} - -// which joints turn the tool, and what one turn of each is worth in its own -// units. Returns the count, or -1 if a joint moves the tool without having a -// period, which the search has no way to bound. -static int tfs_survey(tfs_ctx *c) -{ - double base_axis[3], base_x[3], axis[3], xdir[3]; - static const double candidate[2] = { 360.0, 2*PM_PI }; - int i, k, n = 0; - - for (i = 0; i < c->num_joints; i++) { c->joint[i] = c->seed[i]; } - if (tfs_frame(c, c->joint, base_axis, base_x)) { return -1; } - - for (i = 0; i < c->num_joints; i++) { - double moved = 0; - int p; - - for (k = 0; k < c->num_joints; k++) { c->joint[k] = c->seed[k]; } - c->joint[i] = c->seed[i] + 1e-4; - if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } - for (k = 0; k < 3; k++) { - if (fabs(axis[k] - base_axis[k]) > moved) { moved = fabs(axis[k] - base_axis[k]); } - if (fabs(xdir[k] - base_x[k]) > moved) { moved = fabs(xdir[k] - base_x[k]); } - } - if (moved <= TFS_MOVED_TOL) { continue; } - - if (n >= TOOL_FRAME_MAX_FREE) { return -1; } - - c->scale[n] = 0; - for (p = 0; p < 2; p++) { - double back = 0; - c->joint[i] = c->seed[i] + candidate[p]; - if (tfs_frame(c, c->joint, axis, xdir)) { return -1; } - for (k = 0; k < 3; k++) { - if (fabs(axis[k] - base_axis[k]) > back) { back = fabs(axis[k] - base_axis[k]); } - if (fabs(xdir[k] - base_x[k]) > back) { back = fabs(xdir[k] - base_x[k]); } - } - if (back <= TFS_MOVED_TOL) { - c->scale[n] = candidate[p]/(2*PM_PI); - break; - } - } - if (c->scale[n] == 0) { return -1; } - - c->free[n] = i; - n++; - } - c->nfree = n; - return n; -} - -// enumerate the roots for whatever the context currently constrains -static int tfs_search(tfs_ctx *c, - double *solutions, - int max_solutions, - int *free_directions) -{ - double kept[TOOL_FRAME_MAX_SOLUTIONS][TOOL_FRAME_MAX_FREE]; - double u[TOOL_FRAME_MAX_FREE], useed[TOOL_FRAME_MAX_FREE]; - double r[TFS_MAX_RES], J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE]; - int index[TOOL_FRAME_MAX_FREE]; - int found = 0, per_axis, first = 1, i, k; - - for (i = 0; i < TOOL_FRAME_MAX_FREE; i++) { u[i] = 0; useed[i] = 0; } - - // nothing on this machine turns the tool, so the only candidate is where - // the machine already is - if (c->nfree == 0) { - if (tfs_res(c, u, r)) { return -1; } - if (tfs_norm2(r, c->nres) > TFS_SOLVED) { return 0; } - for (i = 0; i < c->num_joints; i++) { solutions[i] = c->seed[i]; } - if (free_directions) { free_directions[0] = 0; } - return 1; - } - - for (i = 0; i < c->nfree; i++) { - useed[i] = c->seed[c->free[i]] / c->scale[i]; - index[i] = 0; - } - - // Quarter turns of each free joint, starting from where the machine is so - // that a machine with a free direction reports the answer nearest its - // present pose. Two per turn already enters every basin on the machines - // in the tree, and four is the margin for one that is not: the roots are - // few and widely separated, because they come from the two branches of an - // arc cosine and not from anything finely structured. - per_axis = 4; - - for (;;) { - int solved, rank, dup = 0; - - if (first) { - for (i = 0; i < c->nfree; i++) { u[i] = useed[i]; } - } else { - for (i = 0; i < c->nfree; i++) { - u[i] = -PM_PI + (2*PM_PI*index[i])/per_axis; - } - } - - solved = tfs_levmar(c, u); - if (solved) { - for (i = 0; i < c->nfree; i++) { u[i] = tfs_wrap(u[i]); } - if (tfs_res(c, u, r) || tfs_jac(c, u, J)) { return -1; } - - rank = tfs_rank((const double (*)[TOOL_FRAME_MAX_FREE])J, - c->nres, c->nfree); - tfs_joints(c, u); - - // a rank deficient root means the request does not pin the machine - // down and the answer is a continuum. Report this one point of it - // and say so, rather than returning samples of a curve alongside - // roots that mean something else. - if (c->nfree - rank > 0) { - for (i = 0; i < c->num_joints; i++) { solutions[i] = c->joint[i]; } - if (free_directions) { free_directions[0] = c->nfree - rank; } - return 1; - } - - // Two roots are the same pose if going from one to the other - // does not move the tool. That covers landing on a root already - // found, and it also covers the case a distance test would get - // wrong: near a singularity the search reaches points a long way - // apart in joint values whose frames differ by less than it can - // resolve, and those are one answer and not several. - for (k = 0; k < found; k++) { - double mid[TOOL_FRAME_MAX_FREE] = {0}; - - for (i = 0; i < c->nfree; i++) { - mid[i] = kept[k][i] + tfs_wrap(u[i] - kept[k][i])/2; - } - if (tfs_res(c, mid, r)) { return -1; } - if (tfs_norm2(r, c->nres) <= TFS_SOLVED) { dup = 1; break; } - } - - if (!dup) { - // the dedupe evaluated other points, so rebuild this one - tfs_joints(c, u); - for (i = 0; i < c->num_joints; i++) { - solutions[found*c->num_joints + i] = c->joint[i]; - } - if (free_directions) { free_directions[found] = 0; } - for (i = 0; i < c->nfree; i++) { kept[found][i] = u[i]; } - found++; - if (found >= max_solutions) { return found; } - } - } - - if (first) { first = 0; continue; } - - for (i = 0; i < c->nfree; i++) { - if (++index[i] < per_axis) { break; } - index[i] = 0; - } - if (i == c->nfree) { break; } - } - - return found; -} - -// the turn about the tool axis that carries the tool x this pose achieves onto -// the one the caller asked for -static int tfs_spin(tfs_ctx *c, const double *joint, - const PmCartesian *x_in_work, double *spin) -{ - KINEMATICS_FORWARD_FLAGS fflags = 0; - PmRotationMatrix w, t, m; - double along_x, along_y; - - if (c->work(joint, &w, &fflags)) { return -1; } - if (c->tool(joint, &t, &fflags)) { return -1; } - toolFrameInWork(&w, &t, &m); - - along_x = m.x.x*x_in_work->x + m.x.y*x_in_work->y + m.x.z*x_in_work->z; - along_y = m.y.x*x_in_work->x + m.y.y*x_in_work->y + m.y.z*x_in_work->z; - - *spin = atan2(along_y, along_x); - return 0; -} - -int toolFrameSolve(kinsFrameFunc work, - kinsFrameFunc tool, - int num_joints, - const PmCartesian *axis_in_work, - const PmCartesian *x_in_work, - const double *seed, - double *solutions, - int max_solutions, - int *free_directions, - double *tool_spin) -{ - tfs_ctx c; - int found, i; - - if (!work || !tool || !seed || !solutions || !axis_in_work - || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS - || max_solutions <= 0) { - return -1; - } - if (max_solutions > TOOL_FRAME_MAX_SOLUTIONS) { - max_solutions = TOOL_FRAME_MAX_SOLUTIONS; - } - - c.work = work; - c.tool = tool; - c.num_joints = num_joints; - c.seed = seed; - c.nres = x_in_work ? 6 : 3; - c.want[0] = axis_in_work->x; - c.want[1] = axis_in_work->y; - c.want[2] = axis_in_work->z; - if (x_in_work) { - double square = axis_in_work->x * x_in_work->x - + axis_in_work->y * x_in_work->y - + axis_in_work->z * x_in_work->z; - - // the two vectors are two axes of one frame, so a request where they - // are not at right angles is not a frame and cannot be reached by - // anything - if (fabs(square) > 1e-6) { return -1; } - - c.want[3] = x_in_work->x; - c.want[4] = x_in_work->y; - c.want[5] = x_in_work->z; - } - - if (tfs_survey(&c) < 0) { return -1; } - - found = tfs_search(&c, solutions, max_solutions, free_directions); - if (found != 0 || !x_in_work) { - if (tool_spin) { - for (i = 0; i < (found > 0 ? found : 0); i++) { tool_spin[i] = 0; } - } - return found; - } - - // The joints cannot place tool x, which is the ordinary case: a five axis - // machine spends both rotaries reaching the tool axis and the turn about - // that axis is not a joint at all. It is still reachable, as a rotation - // of the frame rather than a motion of the machine, so answer with the - // poses that reach the axis and the turn that finishes the job. That is - // what a control does with a Heidenhain base vector or a Fanuc G68.2 - // block, neither of which refuses the program for asking. - if (!tool_spin) { return 0; } - - c.nres = 3; - found = tfs_search(&c, solutions, max_solutions, free_directions); - if (found <= 0) { return found; } - - for (i = 0; i < found; i++) { - if (tfs_spin(&c, solutions + i*num_joints, x_in_work, &tool_spin[i])) { - return -1; - } - } - return found; -} - -//---------------------------------------------------------------------- -// The Jacobian. See kinematics.h for what it is and which way it points. -//---------------------------------------------------------------------- - -static void kj_zero(double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) -{ - int j, a; - for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { - for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } - } -} - -// pose coordinate a of p, in EmcPose order -static double *kj_coord(EmcPose *p, int a) -{ - switch (a) { - 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; - } -} - -int kinsJacobianFromInverse(kinsInverseFunc inverse, - int num_joints, - const double *joint, - const EmcPose *world, - const KINEMATICS_INVERSE_FLAGS *iflags, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) -{ - double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; - KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; - KINEMATICS_FORWARD_FLAGS ffl = 0; - EmcPose p; - int j, a; - - if (!inverse || !joint || !world || !jac - || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { - return -1; - } - - kj_zero(jac); - - for (a = 0; a < EMCMOT_MAX_AXIS; a++) { - p = *world; - // the joint array every call sees starts at the machine's own - // position, for a module that reads it before writing it - for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } - - *kj_coord(&p, a) += KINS_JACOBIAN_STEP; - if (inverse(&p, qp, &ifl, &ffl)) { return -1; } - - *kj_coord(&p, a) -= 2 * KINS_JACOBIAN_STEP; - if (inverse(&p, qm, &ifl, &ffl)) { return -1; } - - for (j = 0; j < num_joints; j++) { - jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); - } - } - return 0; -} // kinsJacobianFromInverse() - -int kinsJacobianFromMappedAxes(int max_joints, - const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) -{ - int jno, a; - - if (!map_initialized) { - rtapi_print_msg(RTAPI_MSG_ERR, - "kinsJacobianFromMappedAxes before map_initialized\n"); - return -1; - } - if (max_joints <= 0 || max_joints > EMCMOT_MAX_JOINTS) { return -1; } - - kj_zero(jac); - - for (jno = 0; jno < max_joints; jno++) { - int bit = 1< - - 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 - -#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}; -static KT ktools[SWITCHKINS_MAX_TYPES] = {NULL}; -static KT kworks[SWITCHKINS_MAX_TYPES] = {NULL}; -static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL}; -static KJ kjacs[SWITCHKINS_MAX_TYPES] = {NULL}; -static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; - -// 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() - -int kinematicsToolFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - int r; - - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type]) { - return -1; // this type does not supply one; not an error - } - r = ktools[switchkins_type](joint, rot, fflags); - if (r) { return r; } - - // the type answers in its own frame; put it in the convention here so - // no module has to get the half turn right for itself - return toolFrameApplyNative(rot, &knative[switchkins_type]); -} // kinematicsToolFrame() - -int kinematicsWorkFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !kworks[switchkins_type]) { - return -1; // this type does not supply one; not an error - } - // no native rotation here: the work frame has no tool axis to point the - // wrong way, so there are not two conventions for it to be caught between - return kworks[switchkins_type](joint, rot, fflags); -} // kinematicsWorkFrame() - -int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, - const PmCartesian *x_in_work, - const double *seed, - double *solutions, - int max_solutions, - int *free_directions, - double *tool_spin) -{ - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type] - || !kworks[switchkins_type]) { - return -1; // this type does not report its frames, so it cannot answer - } - - // a type that derived the answer by hand knows its own degenerate poses - // and is faster than a search, so it wins where it exists - if (ktinvs[switchkins_type]) { - return ktinvs[switchkins_type](axis_in_work, x_in_work, seed, - solutions, max_solutions, - free_directions, tool_spin); - } - - // the dispatch itself is what the search calls, so the native rotation - // and the per-type lookup are already accounted for - return toolFrameSolve(kinematicsWorkFrame, kinematicsToolFrame, - kp.max_joints, - axis_in_work, x_in_work, seed, - solutions, max_solutions, free_directions, - tool_spin); -} // kinematicsToolFrameInverse() - -int kinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) -{ - if (switchkins_type < 0 || switchkins_type >= kins_count) { - return -1; - } - // a closed form is exact and knows its own singular poses - if (kjacs[switchkins_type]) { - return kjacs[switchkins_type](joint, world, jac, iflags); - } - // otherwise the type's own inverse, differenced. The type function - // rather than the dispatch, so this cannot recurse through a switch. - if (!kinvs[switchkins_type]) { return -1; } - return kinsJacobianFromInverse(kinvs[switchkins_type], kp.max_joints, - joint, world, iflags, jac); -} // kinematicsJacobian() - -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() - -int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, - const PmRotationMatrix *native) -{ - if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { - rtapi_print_msg(RTAPI_MSG_ERR, - "switchkinsRegisterFrames: BAD switchkins_type <%d>" - " (must be 0..%d)\n", - ktype, SWITCHKINS_MAX_TYPES - 1); - register_error = 1; - return -1; - } - // check the declared rotation once here rather than on every call - if (!native || !toolFrameIsProper(native)) { - rtapi_print_msg(RTAPI_MSG_ERR, - "switchkinsRegisterFrames: switchkins-type %d" - " declared a rotation that is not orthonormal with" - " determinant +1\n", ktype); - register_error = 1; - return -1; - } - kworks[ktype] = kwork; - ktools[ktype] = ktool; - knative[ktype] = *native; - return 0; -} // switchkinsRegisterFrames() - -int switchkinsRegisterJacobian(int ktype, KJ kjac) -{ - if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { - rtapi_print_msg(RTAPI_MSG_ERR, - "switchkinsRegisterJacobian: BAD switchkins_type" - " <%d> (must be 0..%d)\n", - ktype, SWITCHKINS_MAX_TYPES - 1); - register_error = 1; - return -1; - } - kjacs[ktype] = kjac; - return 0; -} // switchkinsRegisterJacobian() - -int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) -{ - if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { - rtapi_print_msg(RTAPI_MSG_ERR, - "switchkinsRegisterToolFrameInverse: BAD" - " switchkins_type <%d> (must be 0..%d)\n", - ktype, SWITCHKINS_MAX_TYPES - 1); - register_error = 1; - return -1; - } - ktinvs[ktype] = kinv; - return 0; -} // switchkinsRegisterToolFrameInverse() - -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); -EXPORT_SYMBOL(kinematicsToolFrameInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(switchkinsRegister); -EXPORT_SYMBOL(switchkinsRegisterFrames); -EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); -EXPORT_SYMBOL(switchkinsRegisterJacobian); -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;} - - // an identity type answers the tool frame the same way whichever module - // asked for it, so supply it here rather than in every switchkinsSetup() - for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { - if (!ktools[i] && kfwds[i] == identityKinematicsForward) { - kworks[i] = identityKinematicsWorkFrame; - ktools[i] = identityKinematicsToolFrame; - knative[i] = TOOL_FRAME_SPINDLE; - } - // and its Jacobian is exact, so do not difference for it - if (!kjacs[i] && kfwds[i] == identityKinematicsForward) { - kjacs[i] = identityKinematicsJacobian; - } - } - - // 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 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 0256c79259b16fad63f5167bfd201bc5a11476d0 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 41/58] 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 dd6e0fbd26bb1c677bb97de9aaeed36fcc11501f 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 42/58] 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 hal_get_real() reads. So a second, non-RT copy of the module can point its haldata at cells carrying the same values the RT instance reads, and its forward and inverse then work unmodified, on live values, at any pose asked 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 existing forward and inverse; - split that coordinate parse out of setup so nonrt_attach() can call it without creating pins. The kinematics math is untouched, and there is no parameter struct to declare, no shared header that grows once per module, no snapshot to refresh inside the servo loop, no sequence counter to get right. Out-of-tree modules can opt in without anyone editing a header they do not own. The cells bound to are pins of the caller's own component, not the RT instance's: a reference into somebody else's pin has a lifetime that belongs to that component, and rewiring the pin would strand it. The loader creates one input pin per value and connects it to the signal the RT pin reads, or, where there is none, to one it makes and removes again on teardown. Such a pin's reference must live in HAL shared memory, since that is where HAL rewrites it on connect and disconnect, so the pins are made against hal_malloc() cells between hal_init() and hal_ready(). Name lookup lives in the loader, userspace code linked against liblinuxcnchal and free to call hal_getref_p(). The module is an RT object, and walking the HAL name space from one is what the HAL isolation work is removing; it would also risk binding against rtlib's copy of the same symbols. No HAL change is needed: this patch touches no file under src/hal. Bind input pins only, or the two copies write to each other's state. 5axiskins has no output pins and no scratch storage, so its non-RT haldata is one static struct. trivkins needs no binding at all, only a statement that joints are axes. 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 what was set while the snapshot version reads the setup default, a snapshot refreshing only when RT calls forward or inverse. With the pin netted, 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 | 95 +++++ src/emc/kinematics/trivkins.c | 15 + src/emc/kinematics_userspace/Submakefile | 1 + .../kinematics_userspace/kinematics_user.c | 381 ++++++++++++++++++ .../kinematics_userspace/kinematics_user.h | 197 +++++++++ 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 | 269 +++++++++++++ 14 files changed, 1959 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 cf590d982a8..ea3cbd53b7b 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; @@ -200,11 +201,20 @@ static int fiveaxis_KinematicsJacobian(const double *joints, jac); } // fiveaxis_KinematicsJacobian() -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); @@ -253,6 +263,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; @@ -281,11 +305,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"); @@ -314,3 +334,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..ed564f21b6f --- /dev/null +++ b/src/emc/kinematics/nonrt_kins.h @@ -0,0 +1,95 @@ +/******************************************************************** + * 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 and a resolver callback. The module names + * each of the pins it reads, keeps the references the resolver + * returns in its own haldata, and hands back its existing forward + * and inverse. The kinematics code itself does not change. + * + * A reference does not point into the RT instance's pin. The + * resolver creates an input pin on the caller's own component and + * connects it to the signal the RT pin reads, so the reference + * belongs to the caller and rewiring cannot strand it. + * + * Name lookup belongs to the caller, userspace code linked against + * liblinuxcnchal. This file is compiled into an RT module, which + * has no business walking the HAL name space and would risk binding + * against rtlib's copy of the same symbols. + * + * Resolve input pins only. Output pins and scratch storage stay + * private to the non-RT copy, or the two copies write to each + * other's state. + * + * 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. Finds 'pin_name' in HAL, checks that it has + type 'type', and writes to 'out' a reference carrying that pin's + value. The reference is to storage the caller owns, not to the named + pin itself. 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 f04d9642622..0690aa9ee39 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] @@ -110,3 +111,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..69abd527dac --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -0,0 +1,381 @@ +/******************************************************************** + * Description: kinematics_user.c + * Non-RT loader for kinematics modules + * + * Loads a kinematics .so with dlopen and calls the nonrt_attach() it + * exports, so this process evaluates the kinematics the machine is + * running, at whatever poses it likes. See nonrt_kins.h. + * + * Identity kinematics needs no module code: the module says so through + * nonrt_ops_t and this file maps joints to axes directly. A module + * exporting no nonrt_attach() is not an error either; the context comes + * back flagged rt_only. + * + * 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); + +/* One per value a kinematics module reads is a generous bound. */ +#define MAX_MADE_SIGNALS 16 +#define MAX_BOUND_PINS 16 + +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]; + int comp_id; /* the caller's component, owns the pins made here */ + const char *prefix; /* its name, which those pin names start with */ + char made_signal[MAX_MADE_SIGNALS][HAL_NAME_LEN + 1]; + int num_made_signals; + hal_refs_u *cell; /* HAL storage those pins are made against */ + int num_cells; +}; + +/* ======================================================================== + * Pin binding + * ======================================================================== */ + +/* + * Give a kinematics module a reference to a value it asked for. + * + * The reference is to a pin of ours rather than into the RT instance's, + * so that its lifetime is ours: see nonrt_kins.h. Ours is connected to + * the signal the RT pin reads, or, when the RT pin has no signal, to one + * made here and removed again in kinematicsUserFree(). + * + * The reference has to live in HAL shared memory, since that is where + * HAL rewrites it on connect and disconnect, so the pins are made + * against hal_malloc() cells and the module gets what a cell holds once + * the connection is in place. + */ +static int make_signal(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type, char *out, size_t outlen) +{ + if (ctx->num_made_signals >= MAX_MADE_SIGNALS) { + fprintf(stderr, "kinematicsUserInit: too many signals to create\n"); + return -1; + } + if ((size_t)snprintf(out, outlen, "%s-nonrt", pin_name) >= outlen) { + fprintf(stderr, "kinematicsUserInit: signal name for '%s' too long\n", + pin_name); + return -1; + } + if (hal_signal_new(out, type) != 0) return -1; + if (hal_link(pin_name, out) != 0) { + hal_signal_delete(out); + return -1; + } + snprintf(ctx->made_signal[ctx->num_made_signals++], + sizeof(ctx->made_signal[0]), "%s", out); + return 0; +} + +static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, + const char *name) +{ + switch (type) { + case HAL_BIT: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); + case HAL_FLOAT: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); + case HAL_S32: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_U32: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); + case HAL_S64: return hal_pin_new_sint(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_U64: return hal_pin_new_uint(comp_id, HAL_IN, &out->u, 0, "%s", name); + default: break; + } + return -1; +} + +static int bind_pin(const char *pin_name, hal_type_t type, + hal_refs_u *out, void *arg) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)arg; + char signal[HAL_NAME_LEN + 1]; + char mine[HAL_NAME_LEN + 1]; + hal_refs_u *cell; + hal_query_t q; + + if (!ctx || !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; + } + + if (q.pp.signal) { + snprintf(signal, sizeof(signal), "%s", q.pp.signal); + } else if (make_signal(ctx, pin_name, type, signal, sizeof(signal))) { + fprintf(stderr, "kinematicsUserInit: cannot reach '%s'\n", pin_name); + return -1; + } + + if ((size_t)snprintf(mine, sizeof(mine), "%s.%s", ctx->prefix, pin_name) + >= sizeof(mine)) { + fprintf(stderr, "kinematicsUserInit: pin name for '%s' too long\n", + pin_name); + return -1; + } + if (ctx->num_cells >= MAX_BOUND_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); + return -1; + } + cell = &ctx->cell[ctx->num_cells++]; + + if (new_pin(ctx->comp_id, type, cell, mine) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); + return -1; + } + if (hal_link(mine, signal) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot link '%s' to '%s'\n", + mine, signal); + return -1; + } + + *out = *cell; + return 0; +} + +/* ======================================================================== + * 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, bind_pin, ctx) != 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, + int comp_id, + const char* prefix) +{ + KinematicsUserContext *ctx; + + if (!kins_type || num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS + || comp_id < 0 || !prefix) { + fprintf(stderr, "kinematicsUserInit: invalid arguments\n"); + return NULL; + } + + ctx = (KinematicsUserContext *)calloc(1, sizeof(KinematicsUserContext)); + if (!ctx) return NULL; + + ctx->num_joints = num_joints; + ctx->comp_id = comp_id; + ctx->prefix = prefix; + + ctx->cell = (hal_refs_u *)hal_malloc(MAX_BOUND_PINS * sizeof(hal_refs_u)); + if (!ctx->cell) { + fprintf(stderr, "kinematicsUserInit: out of HAL memory\n"); + free(ctx); + return NULL; + } + 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) +{ + int i; + + if (!ctx) return; + + /* Removing one hands its value back to the RT pin, leaving the + machine as it was found. */ + for (i = 0; i < ctx->num_made_signals; i++) { + hal_signal_delete(ctx->made_signal[i]); + } + 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..d01d8a8d277 --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -0,0 +1,197 @@ +/******************************************************************** + * 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 kinematics module is loaded into this process and given input pins + * belonging to the caller's HAL component, connected to the same signals + * the running RT instance reads. Its own forward and inverse then work on + * live values, unmodified. + * + * 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 */ +#include /* hal_type_t, HAL_NAME_LEN */ + +#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 + * + * The pins this creates belong to the caller's component, so call this + * after hal_init() and before hal_ready(): HAL refuses new pins once a + * component is ready. + * + * @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") + * @param comp_id Caller's HAL component, from hal_init() + * @param prefix Its name, which the created pin names start with + * @return Allocated context, or NULL if kinematics type not supported + */ +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix); + +/** + * 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) + * + * The bound pins read the live values, so there is nothing to fetch. + * 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 + * + * An RT-only module exports no nonrt_attach() and so cannot be evaluated + * outside RT. Planner 2 is unavailable for such modules. + * + * @param ctx Kinematics context + * @return 1 if RT-only (planner 2 unavailable), 0 if the module is bound + */ +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..1b750be6776 --- /dev/null +++ b/src/emc/motion_planning/kinslimits.cc @@ -0,0 +1,269 @@ +/******************************************************************** + * 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, + comp_id, "kinslimits"); + 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 bea5bcef663a5212e15008db3ef121907c4ba9b1 Mon Sep 17 00:00:00 2001 From: david mueller Date: Sun, 23 Aug 2026 13:32:45 +1000 Subject: [PATCH 43/58] 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 cdc361aeadbb6f505f530f7013a3fedc1b8259fa 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 44/58] 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 41afe43a2405a2dde73c0b11eabba0c2e8f93e7c 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 45/58] 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 From 07d649a249475e1a4d8a6a35eecb27cd021f6d24 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:44:16 +1000 Subject: [PATCH 46/58] kinematics: add the parameter block form of a module A kinematics module reads its geometry from HAL pins it created, keeps its kinematics type and iteration scratch in statics, and so can only answer for the machine as it is now, from inside the module. Anything else that needs the same maths, a planner evaluating poses the machine has not reached, task checking a program at load, a tool asking what if, has to carry a second copy of it. Add the form in which the caller supplies everything: a kins_params block naming the kinematics type, the joint map, the tool and the geometry, a kins_scratch for what an iterative method carries between calls, and a kins_ops table holding the forward, inverse, frames and Jacobian of one type as functions of the two. A module declares its geometry as a table of named entries. In RT the shared code makes one pin per entry, with the names configs already use, copies the pins into the block before every call and the declared outputs back after it, so the maths never touches a pin. Outside RT the caller fills the block from wherever it likes. The classic entry points stay and are supplied once: kins_single.c for a module with one type, switchkins.c for one with several, which now dispatches a type registered with switchkinsRegisterOps() through the block and the older registrations as before, so a module converts one type at a time. Every converted module exports kinsDescribe(), which hands a caller outside RT its table and the ops of each type; a module that does not provide the form keeps working unchanged and reports itself RT-only. Convert trivkins, 5axiskins and the userkfuncs template, and move the non-RT loader onto kinsDescribe(): it binds one input pin per table entry the way it bound the pins nonrt_attach() asked for, fills the block, and evaluates through the same kinsOps functions the RT wrappers use, so both sides get the same answers. nonrt_attach() and its header go, along with the second haldata each module had to keep for it. The loader also takes the tool from motion.tooloffset.* where motion is loaded, and says once when the module's own tool pin disagrees with it, which is a config that lost the tool on the way. jacobian.cc asks the module for its Jacobian instead of differencing at a 0.1 step. switchkinsSetup() is now run by switchkinsRunSetup() in switchkins_setup.c, which a module links only if it defines switchkinsSetup(), so the halcompile components keep linking the core without one. The kparms they build are zeroed, since the struct has grown. identityKinematicsSetup() no longer asks kinematicsType() when built outside RT, where there is no module around it. kinslimits reports the same caps for 5axiskins as before to the digit, now from the closed form Jacobian. tests/kins-jacobian, kins-frames, tool-frame and matrixkins pass unchanged. --- src/Makefile | 11 +- src/emc/kinematics/5axiskins.c | 255 +++------- src/emc/kinematics/kinematics.h | 214 ++++++++ src/emc/kinematics/kins_rt.h | 57 +++ src/emc/kinematics/kins_single.c | 155 ++++++ src/emc/kinematics/kins_util.c | 473 +++++++++++++++++- src/emc/kinematics/nonrt_kins.h | 95 ---- src/emc/kinematics/switchkins.c | 226 ++++++++- src/emc/kinematics/switchkins.h | 26 +- src/emc/kinematics/switchkins_main.c | 42 +- src/emc/kinematics/switchkins_setup.c | 85 ++++ src/emc/kinematics/trivkins.c | 126 ++--- src/emc/kinematics/userkfuncs.c | 34 ++ .../kinematics_userspace/kinematics_user.c | 363 ++++++++++---- .../kinematics_userspace/kinematics_user.h | 61 ++- src/emc/motion_planning/Submakefile | 4 +- src/emc/motion_planning/jacobian.cc | 121 +---- src/emc/motion_planning/jacobian.hh | 24 +- src/hal/components/millturn.comp | 2 +- src/hal/components/xyzab_tdr_kins.comp | 2 +- src/hal/components/xyzacb_trsrn.comp | 2 +- src/hal/components/xyzbca_trsrn.comp | 2 +- 22 files changed, 1674 insertions(+), 706 deletions(-) create mode 100644 src/emc/kinematics/kins_rt.h create mode 100644 src/emc/kinematics/kins_single.c delete mode 100644 src/emc/kinematics/nonrt_kins.h create mode 100644 src/emc/kinematics/switchkins_setup.c diff --git a/src/Makefile b/src/Makefile index 37f0c6664ad..eb6044048fc 100644 --- a/src/Makefile +++ b/src/Makefile @@ -404,7 +404,7 @@ SRCHEADERS := \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ emc/kinematics/switchkins.h \ - emc/kinematics/nonrt_kins.h \ + emc/kinematics/kins_rt.h \ emc/kinematics_userspace/kinematics_user.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/inifile.hh \ @@ -1141,6 +1141,7 @@ hal_lib-objs := hal/hal_lib.o $(MATHSTUB) obj-m += trivkins.o trivkins-objs := emc/kinematics/trivkins.o trivkins-objs += emc/kinematics/kins_util.o +trivkins-objs += emc/kinematics/kins_single.o obj-m += maxkins.o maxkins-objs := emc/kinematics/maxkins.o @@ -1196,6 +1197,7 @@ 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 += emc/kinematics/switchkins_setup.o genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o @@ -1206,6 +1208,7 @@ 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 += emc/kinematics/switchkins_setup.o genserkins-objs += $(USERKFUNCS) obj-m += xyzac-trt-kins.o @@ -1214,6 +1217,7 @@ 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 += emc/kinematics/switchkins_setup.o xyzac-trt-kins-objs += $(USERKFUNCS) obj-m += xyzbc-trt-kins.o @@ -1222,6 +1226,7 @@ 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 += emc/kinematics/switchkins_setup.o xyzbc-trt-kins-objs += $(USERKFUNCS) obj-m += scarakins.o @@ -1231,6 +1236,7 @@ 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 += emc/kinematics/switchkins_setup.o scarakins-objs += $(USERKFUNCS) obj-m += pumakins.o @@ -1240,6 +1246,7 @@ 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 += emc/kinematics/switchkins_setup.o pumakins-objs += $(USERKFUNCS) obj-m += three21kins.o @@ -1249,6 +1256,7 @@ 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 += emc/kinematics/switchkins_setup.o three21kins-objs += $(USERKFUNCS) obj-m += 5axiskins.o @@ -1258,6 +1266,7 @@ obj-m += 5axiskins.o 5axiskins-objs += emc/kinematics/kins_util.o 5axiskins-objs += emc/kinematics/switchkins.o 5axiskins-objs += emc/kinematics/switchkins_main.o +5axiskins-objs += emc/kinematics/switchkins_setup.o 5axiskins-objs += $(USERKFUNCS) #---------------------------------------------------------------- diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index ea3cbd53b7b..88ac895b0dd 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -42,8 +42,8 @@ * 9) Coordinates XYZBCW are required, AUV may be used * if specified with the coordinates parameter and will * be mapped one-to-one with the assigned joint. -* 10) The direction of the tilt axis is the opposite of the -* conventional axis direction. See +* 10) The direction of the tilt axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -56,17 +56,28 @@ #include #include #include -#include #include #include #include -#include -static struct haldata { - hal_real_t pivot_length; -} *haldata; -static int fiveaxis_max_joints; +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, +}; +enum { P_PIVOT_LENGTH }; + +// assignments of principal joints to axis letters, from the block +// (-1 means not defined) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) static PmCartesian s2r(double r, double t, double p) { // s2r: spherical coordinates to cartesian coordinates @@ -84,26 +95,16 @@ static PmCartesian s2r(double r, double t, double p) { return c; } //s2r() -// assignments of principal joints to axis letters: -// (-1 means not defined (yet)) -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; - -static int fiveaxis_KinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int fiveaxis_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + double pivot_length = p->geometry[P_PIVOT_LENGTH]; PmCartesian r = s2r(pivot_length + joints[JW], joints[JC], 180.0 - joints[JB]); @@ -122,16 +123,18 @@ static int fiveaxis_KinematicsForward(const double *joints, pos->v = (JV != -1)? joints[JV] : 0; return 0; -} //fiveaxis_KinematicsForward() +} // fiveaxis_forward() -static int fiveaxis_KinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int fiveaxis_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + double pivot_length = p->geometry[P_PIVOT_LENGTH]; PmCartesian r = s2r(pivot_length + pos->w, pos->c, 180.0 - pos->b); @@ -153,21 +156,18 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(fiveaxis_max_joints, - &P, - joints); - return 0; -} // fiveaxis_kinematicsInverse() - -static int fiveaxis_KinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) + return kinsPoseToMappedJoints(p, &P, joints); +} // fiveaxis_inverse() + +static int fiveaxis_jacobian(const kins_params *p, + const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)joints; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - const double R = pivot_length + pos->w; + const double R = p->geometry[P_PIVOT_LENGTH] + pos->w; const double sb = sin(TO_RAD*pos->b), cb = cos(TO_RAD*pos->b); const double sc = sin(TO_RAD*pos->c), cc = cos(TO_RAD*pos->c); double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; @@ -196,108 +196,15 @@ static int fiveaxis_KinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(fiveaxis_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // fiveaxis_KinematicsJacobian() - -// 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 i,jno; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int minjoints = strlen(kp->required_coordinates); - fiveaxis_max_joints = strlen(coordinates); // allow for dup coords - - if (fiveaxis_max_joints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: coordinates=%s requires %d joints, max joints=%d\n", - kp->kinsname, - coordinates, - fiveaxis_max_joints, - kp->max_joints); - goto error; - } - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coordinates (order doesn't matter) - for (i=0; i < minjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jnopivot_length), - DEFAULT_PIVOT_LENGTH, "%s.pivot-length", kp->halprefix); - if(result < 0) goto error; - - rtapi_print("Kinematics Module %s\n",__FILE__); - rtapi_print(" module name = %s\n" - " coordinates = %s Requires: [KINS]JOINTS>=%d\n" - " sparm = %s\n", - kp->kinsname, - coordinates,fiveaxis_max_joints, - kp->sparm?kp->sparm:"NOTSPECIFIED"); - rtapi_print(" default pivot-length = %.3f\n", hal_get_real(haldata->pivot_length)); - - return 0; - -error: - return -1; -} // fiveaxis_KinematicsSetup() +static const kins_ops fiveaxis_ops = { + .forward = fiveaxis_forward, + .inverse = fiveaxis_inverse, + .jacobian = fiveaxis_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -305,57 +212,27 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { - fiveaxis_kparms(kp); + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)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; + kp->params = fiveaxis_params; + kp->nparams = sizeof(fiveaxis_params)/sizeof(fiveaxis_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = fiveaxis_KinematicsSetup; - *kfwd1 = fiveaxis_KinematicsForward; - *kinv1 = fiveaxis_KinematicsInverse; - switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &fiveaxis_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = fiveaxis_KinematicsSetup; - *kfwd0 = fiveaxis_KinematicsForward; - *kinv0 = fiveaxis_KinematicsInverse; - switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; + switchkinsRegisterOps(0, &fiveaxis_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); 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/kinematics.h b/src/emc/kinematics/kinematics.h index 2a6c6a0b750..839435b90f4 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -173,6 +173,8 @@ typedef struct kinematics_parms { // bitmask: 0x4 bit2: switchkins_type==2 int gui_kinstype; // may be reqd for parallel kins with vismach // to select switchkins_type for gui pins + const struct kins_param_desc_tag *params; // geometry table, see below + int nparams; } kparms; /* map letters in a coordinates string to joint numbers @@ -425,6 +427,216 @@ extern int identityKinematicsJacobian(const double *joint, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags); +/* ------------------------------------------------------------------------ + Kinematics as pure functions of what the caller passes in. + + Everything above reads its geometry from HAL pins the module created and + keeps its mode and scratch in statics, so it can only answer for the + machine as it is now, from inside the module. The forms below take the + same questions with the machine described by the caller: a parameter + block naming the kinematics type, the joint map, the tool and the + geometry, and a scratch block for what an iterative method carries + between calls. Nothing is read from HAL and nothing is kept, so one copy + of the maths serves motion, a planner evaluating poses the machine has + not reached, task checking a program at load, and a tool asking what if. + + A module declares its geometry as a table of named entries. In RT the + shared code makes one HAL pin per entry, with the names configs already + use, and copies the pins into the block before every call; outside RT the + caller fills the block from wherever it likes. The maths reads + p->geometry[i] where it read a pin. + + The existing entry points stay and are supplied once, by kins_single.c + for a module with one kinematics type and by switchkins.c for one with + several, so nothing that calls kinematicsForward() changes. A module + that does not provide these forms keeps working as it did; it just cannot + be evaluated outside RT. + ------------------------------------------------------------------------ */ + +#define KINS_MAX_PARAMS 96 /* genhexkins declares 84 */ +#define KINS_MAX_TYPES 9 /* kinematics types a module may provide */ + +typedef enum { + KINS_PARAM_FLOAT = 0, + KINS_PARAM_BIT, + KINS_PARAM_S32, + KINS_PARAM_U32 +} kins_param_type; + +typedef enum { + KINS_IN = 0, /* read into the block before a call */ + KINS_OUT, /* a result, written from kins_scratch.out[] after it */ + KINS_IO /* read like an input; the pin is HAL_IO so it can be poked */ +} kins_param_dir; + +/* One entry of a module's geometry table. name follows the module's HAL + prefix. An entry with tool set is the tool length along the tool axis: + the shared code puts its value in kins_params.tool.tran.z as well, which + is what the maths should read, so that a caller outside RT can supply + the tool from the tool table without there being a pin. */ +typedef struct kins_param_desc_tag { + const char *name; + kins_param_type type; + kins_param_dir dir; + int tool; + double dflt; +} kins_param_desc; + +/* The machine, as far as the kinematics is concerned. One copy may be + shared by any number of callers: nothing writes it during a call. */ +typedef struct kins_params { + int size; /* sizeof(kins_params) */ + int ktype; /* kinematics type, 0 if one */ + int max_joints; /* joints the map covers */ + int joint_of_axis[EMCMOT_MAX_AXIS]; /* principal joint per letter */ + int joints_of_axis[EMCMOT_MAX_AXIS]; /* bit per joint, duplicates */ + EmcPose tool; /* tool offset, tool.tran.z along the tool axis */ + double geometry[KINS_MAX_PARAMS]; /* the table, in its order */ +} kins_params; + +/* What one caller carries between its own calls: the last pose an + iterative forward found, which seeds the next, and what a module reports + about its last call. Never shared between callers. */ +typedef struct kins_scratch { + EmcPose pose_seed; /* start an iterative forward here */ + int have_pose_seed; + double joint_seed[EMCMOT_MAX_JOINTS]; /* start an iterative inverse here */ + int have_joint_seed; + int iterations; + int failed; + double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ +} kins_scratch; + +typedef int (*kins_forward_fn)(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kins_inverse_fn)(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_frame_fn)(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +/* The maths of one kinematics type. forward and inverse are required; the + frames, the native rotation and the Jacobian are optional as before, and + a missing Jacobian is differenced from the inverse. fwd_iterates says the + forward starts from the pose it is handed, so the shared code seeds it + with the last answer after a switch. identity says joints are axes, which + a consumer may use to skip the maths altogether. */ +typedef struct kins_ops { + kins_forward_fn forward; + kins_inverse_fn inverse; + kins_frame_fn work; + kins_frame_fn tool; + const PmRotationMatrix *native; /* NULL means TOOL_FRAME_SPINDLE */ + kins_jacobian_fn jacobian; + int fwd_iterates; + int identity; /* joints are axes */ +} kins_ops; + +/* A module described for a caller outside RT: its table, its joint + conventions and the maths of each type. ops[t] is NULL for a type the + module still implements the old way. */ +typedef struct kins_module_info { + const char *name; + const char *halprefix; + const kins_param_desc *params; + int nparams; + const char *required_coordinates; + int max_joints; /* the most the module allows */ + int allow_duplicates; + int ntypes; + const kins_ops *ops[KINS_MAX_TYPES]; +} kins_module_info; + +/* Exported by every module that provides the forms above. coordinates and + sparm are the module parameters the RT instance was loaded with; a module + whose types depend on them replays that choice here. Meant for a copy of + the module loaded outside RT; the RT instance answers from its own state + without redoing its setup. Returns 0, or -1 with info untouched. */ +extern int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info); + +/* Fill a block for a module: size, the joint map from coordinates (checked + against required_coordinates, the joint limit and the duplicate rule), + ktype 0, no tool, and every geometry entry at its table default. A + caller then overwrites what it knows better. Returns 0 or -1. */ +extern int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates); + +/* The joint map alone, into a block, with no other field touched. */ +extern int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates); + +/* Reset a scratch to "no seed, nothing reported". */ +extern void kinsScratchInit(kins_scratch *s); + +/* The map helpers above, reading the map from the block instead of from + the statics that map_coordinates_to_jnumbers() fills. */ +extern int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos); +extern int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints); +extern int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* Identity as pure functions: joints are axes through the block's map. */ +extern int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); +extern const kins_ops KINS_IDENTITY_OPS; + +/* The five questions asked of an ops table, with the defaults applied: + identity for a missing frame, the native rotation applied to the tool + frame, and the Jacobian differenced from the inverse when there is no + closed form. These are what the RT wrappers and a caller outside RT + both go through, so both get the same answers. */ +extern int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); //NOTE: switchable kinematics may require Interp::Synch @@ -439,6 +651,8 @@ EXPORT_SYMBOL(kinematicsSwitch); // support for template for user-defined switchkins_type==2 +extern const kins_ops USERK_OPS; + extern int userkKinematicsSetup(const int comp_id, const char* coordinates, kparms* ksetup_parms); diff --git a/src/emc/kinematics/kins_rt.h b/src/emc/kinematics/kins_rt.h new file mode 100644 index 00000000000..96d7309c739 --- /dev/null +++ b/src/emc/kinematics/kins_rt.h @@ -0,0 +1,57 @@ +/******************************************************************** +* Description: kins_rt.h +* The RT side of a kinematics module written as pure functions: the HAL +* pins made from its geometry table, and the wrapper that supplies the +* classic entry points for a module with one kinematics type. A module +* with several types gets the same from switchkins.c. +* +* Kept apart from kinematics.h because everything here needs HAL, and +* kinematics.h is read by callers outside RT that do not. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_KINS_RT_H +#define __LINUXCNC_KINS_RT_H + +#include +#include "kinematics.h" + +/* one HAL pin handle per table entry, of whichever type the entry has */ +typedef union { + hal_real_t r; + hal_bool_t b; + hal_sint_t s; + hal_uint_t u; +} kins_pin_ref; + +/* Make one pin per table entry, named ., inputs at their + defaults. *out receives the handles, from hal_malloc(), or NULL for an + empty table. Returns 0 or -1. */ +extern int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out); + +/* Copy every input pin into p->geometry[], and the tool entry into + p->tool.tran.z as well. */ +extern void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p); + +/* Copy s->out[] to every output pin. */ +extern void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s); + +/* A module with one kinematics type defines this, describing itself, and + links kins_single.c, which supplies kinematicsForward() and the rest + from it. ops[0] is the maths; the other entries are ignored. */ +extern const kins_module_info kins_module; + +/* Called once from the module's rtapi_app_main() or EXTRA_SETUP(), after + hal_init() and before hal_ready(): makes the pins, builds the block for + coordinates and records the KINEMATICS_TYPE that kinematicsType() will + report. Returns 0 or -1. */ +extern int kinsSingleInit(int comp_id, const char *coordinates, + KINEMATICS_TYPE reported); + +#endif diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c new file mode 100644 index 00000000000..58f914076d5 --- /dev/null +++ b/src/emc/kinematics/kins_single.c @@ -0,0 +1,155 @@ +/******************************************************************** +* Description: kins_single.c +* The classic kinematics entry points for a module with one kinematics +* type written as pure functions. The module defines kins_module and +* calls kinsSingleInit(); this file keeps the one RT parameter block, +* fills it from the pins before every call, and hands the call to the +* module's ops. It is the counterpart of switchkins.c for a module that +* does not switch. +* +* License: GPL Version 2 +********************************************************************/ + +#include +#include +#include + +#include +#include + +static kins_params rt_params; +static kins_scratch rt_scratch; +static kins_pin_ref *pins; +static int inited; +static KINEMATICS_TYPE reported_type = KINEMATICS_BOTH; + +static const kins_ops *ops(void) +{ + return inited ? kins_module.ops[0] : NULL; +} + +// the block sees the pins as they are now +static void read_pins(void) +{ + kinsParamsPinsRead(pins, kins_module.params, kins_module.nparams, + &rt_params); +} + +static void write_pins(void) +{ + kinsParamsPinsWrite(pins, kins_module.params, kins_module.nparams, + &rt_scratch); +} + +int kinsSingleInit(int comp_id, const char *coordinates, + KINEMATICS_TYPE reported) +{ + if (!kins_module.ops[0] || !kins_module.ops[0]->forward + || !kins_module.ops[0]->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsSingleInit: %s supplies no forward or inverse\n", + kins_module.name ? kins_module.name : "?"); + return -1; + } + if (kinsParamsInit(&rt_params, &kins_module, coordinates)) { return -1; } + kinsScratchInit(&rt_scratch); + if (kinsParamsPinsCreate(comp_id, kins_module.halprefix, + kins_module.params, kins_module.nparams, + &pins)) { + return -1; + } + reported_type = reported; + inited = 1; + return 0; +} // kinsSingleInit() + +int kinematicsForward(const double *joint, + EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsForward(ops(), &rt_params, &rt_scratch, joint, pos, fflags, iflags); + write_pins(); + return r; +} + +int kinematicsInverse(const EmcPose *pos, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsInverse(ops(), &rt_params, &rt_scratch, pos, joint, iflags, fflags); + write_pins(); + return r; +} + +int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsWorkFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsToolFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsJacobian(const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsJacobian(ops(), &rt_params, &rt_scratch, joint, pos, jac, iflags); +} + +KINEMATICS_TYPE kinematicsType(void) +{ + return reported_type; +} + +int kinematicsSwitchable(void) { return 0; } + +int kinematicsSwitch(int switchkins_type) +{ + (void)switchkins_type; + return 0; +} + +// The module's description, for a copy of it loaded outside RT. A module +// with one type does not depend on its parameters for its shape, so this +// is the table as declared. +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + (void)coordinates; + (void)sparm; + if (!info) { return -1; } + *info = kins_module; + info->ntypes = 1; + return 0; +} + +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSwitchable); +EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 2e30969fd90..6c285f19c91 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -48,7 +48,9 @@ #include #include #include +#include #include +#include // principal joint numbers based on module 'coordinates' parameter static int JX = -1; @@ -76,38 +78,23 @@ 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 +// Letters to joint numbers, in order, with the checks every caller wants: +// a valid letter set, at most max_joints of them, duplicates only where +// allowed. Fills axis_idx_for_jno (-1 past the last letter) and touches +// nothing else, so the block form and the static form share it. +static int kins_scan_coordinates(const char *coordinates, + int max_joints, + int allow_duplicates, + int axis_idx_for_jno[], + const char *errtag) { - char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; - int jno=0; - bool found=0; + 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) ) { @@ -168,6 +155,40 @@ int map_coordinates_to_jnumbers(const char *coordinates, } } } + return 0; +} // kins_scan_coordinates() + +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; + + 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; + } + } + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, errtag)) { + return -1; + } for (jno=0; jno < max_joints; jno++) { int bitnumber = 1< Axis %c\n", jno,*(p+axis_idx_for_jno[jno])); } +#ifndef ULAPI + // the module's own report of its type; this file is also built + // outside RT, where there is no module around it if (kinematicsType() != KINEMATICS_BOTH) { rtapi_print("identityKinematicsSetup: Recommend: kinstype=both\n"); } +#endif rtapi_print("\n"); } @@ -1166,3 +1191,399 @@ int identityKinematicsJacobian(const double *joint, (const double (*)[EMCMOT_MAX_AXIS])dP, jac); } // identityKinematicsJacobian() + +//---------------------------------------------------------------------- +// The parameter block. See kinematics.h for what it is for. +//---------------------------------------------------------------------- + +int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates) +{ + int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; + int jno, a; + + if (!p) { return -1; } + if (!coordinates) { coordinates = "XYZABCUVW"; } + + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, + "kinsParamsMapCoordinates: ERROR:\n ")) { + return -1; + } + + // every letter the module cannot do without has to be there + for (a = 0; required_coordinates && required_coordinates[a]; a++) { + char want = required_coordinates[a]; + const char *c; + int seen = 0; + for (c = coordinates; *c; c++) { + if (*c == want || *c == want + ('a' - 'A') || *c == want - ('a' - 'A')) { + seen = 1; break; + } + } + if (!seen) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsMapCoordinates: ERROR:\n required coordinates:%s\n" + " specified coordinates:%s\n", + required_coordinates, coordinates); + return -1; + } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p->joint_of_axis[a] = -1; + p->joints_of_axis[a] = 0; + } + p->max_joints = 0; + for (jno = 0; jno < EMCMOT_MAX_JOINTS; jno++) { + a = axis_idx_for_jno[jno]; + if (a < 0) { break; } + if (p->joint_of_axis[a] < 0) { p->joint_of_axis[a] = jno; } + p->joints_of_axis[a] |= 1 << jno; + p->max_joints = jno + 1; + } + return 0; +} // kinsParamsMapCoordinates() + +int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates) +{ + int i; + + if (!p || !info) { return -1; } + if (info->nparams < 0 || info->nparams > KINS_MAX_PARAMS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsInit: %s declares %d parameters, at most %d allowed\n", + info->name ? info->name : "?", info->nparams, KINS_MAX_PARAMS); + return -1; + } + + memset(p, 0, sizeof(*p)); + p->size = sizeof(*p); + p->ktype = 0; + if (!coordinates) { coordinates = info->required_coordinates; } + if (kinsParamsMapCoordinates(p, coordinates, info->max_joints, + info->allow_duplicates, + info->required_coordinates)) { + return -1; + } + for (i = 0; i < info->nparams; i++) { + p->geometry[i] = info->params[i].dflt; + if (info->params[i].tool) { p->tool.tran.z = info->params[i].dflt; } + } + return 0; +} // kinsParamsInit() + +void kinsScratchInit(kins_scratch *s) +{ + if (s) { memset(s, 0, sizeof(*s)); } +} + +int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos) +{ + int a; + if (!p || !joints || !pos) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j = p->joint_of_axis[a]; + if (j < 0) { continue; } + switch (a) { + case 0: pos->tran.x = joints[j]; break; + case 1: pos->tran.y = joints[j]; break; + case 2: pos->tran.z = joints[j]; break; + case 3: pos->a = joints[j]; break; + case 4: pos->b = joints[j]; break; + case 5: pos->c = joints[j]; break; + case 6: pos->u = joints[j]; break; + case 7: pos->v = joints[j]; break; + default: pos->w = joints[j]; break; + } + } + return 0; +} // kinsMappedJointsToPose() + +static double kins_pose_coord(const EmcPose *pos, int a) +{ + switch (a) { + case 0: return pos->tran.x; + case 1: return pos->tran.y; + case 2: return pos->tran.z; + case 3: return pos->a; + case 4: return pos->b; + case 5: return pos->c; + case 6: return pos->u; + case 7: return pos->v; + default: return pos->w; + } +} + +int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints) +{ + int a, jno; + if (!p || !pos || !joints) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (bits & (1 << jno)) { joints[jno] = kins_pose_coord(pos, a); } + } + } + return 0; +} // kinsPoseToMappedJoints() + +int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int a, jno, col; + if (!p || !dP || !jac) { return -1; } + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (!(bits & (1 << jno))) { continue; } + for (col = 0; col < EMCMOT_MAX_AXIS; col++) { jac[jno][col] = dP[a][col]; } + } + } + return 0; +} // kinsJacobianFromMappedAxesP() + +//---------------------------------------------------------------------- +// identity through the block +//---------------------------------------------------------------------- + +int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return kinsMappedJointsToPose(p, joint, pos); +} + +int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return kinsPoseToMappedJoints(p, pos, joint); +} + +int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; (void)joint; (void)fflags; + *rot = TOOL_FRAME_SPINDLE; + return 0; +} + +int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + (void)joint; (void)pos; (void)iflags; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = (a == b) ? 1.0 : 0.0; } + } + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, jac); +} + +const kins_ops KINS_IDENTITY_OPS = { + .forward = kinsIdentityForward, + .inverse = kinsIdentityInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = kinsIdentityJacobian, + .fwd_iterates = 0, + .identity = 1, +}; + +//---------------------------------------------------------------------- +// asking an ops table, defaults applied +//---------------------------------------------------------------------- + +int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!ops || !ops->forward || !p || !s) { return -1; } + if (ops->fwd_iterates && s->have_pose_seed) { + *pos = s->pose_seed; + s->have_pose_seed = 0; + } + r = ops->forward(p, s, joint, pos, fflags, iflags); + if (ops->fwd_iterates) { s->pose_seed = *pos; } + return r; +} + +int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !ops->inverse || !p || !s) { return -1; } + return ops->inverse(p, s, pos, joint, iflags, fflags); +} + +int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !p || !rot) { return -1; } + if (!ops->work) { return -1; } // not supplied; not an error + return ops->work(p, joint, rot, fflags); +} + +int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!ops || !p || !rot) { return -1; } + if (!ops->tool) { return -1; } // not supplied; not an error + r = ops->tool(p, joint, rot, fflags); + if (r) { return r; } + return toolFrameApplyNative(rot, ops->native ? ops->native + : &TOOL_FRAME_SPINDLE); +} + +int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose q; + int j, a; + + if (!ops || !p || !s || !joint || !pos || !jac) { return -1; } + if (ops->jacobian) { return ops->jacobian(p, joint, pos, jac, iflags); } + if (!ops->inverse) { return -1; } + + // the same differences as kinsJacobianFromInverse(), on the block form + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + q = *pos; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&q, a) += KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&q, a) -= 2 * KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < p->max_joints && j < EMCMOT_MAX_JOINTS; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsOpsJacobian() + +//---------------------------------------------------------------------- +// the RT side of the table: one HAL pin per entry, copied into the block +// before a call and out of the scratch after it +//---------------------------------------------------------------------- + +int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out) +{ + kins_pin_ref *pins; + int i, res = 0; + + if (!out) { return -1; } + *out = NULL; + if (nparams < 0 || nparams > KINS_MAX_PARAMS) { return -1; } + if (nparams == 0) { return 0; } + if (!params || !prefix) { return -1; } + + pins = hal_malloc(nparams * sizeof(*pins)); + if (!pins) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: hal_malloc failed\n"); + return -1; + } + for (i = 0; i < nparams; i++) { + const kins_param_desc *d = ¶ms[i]; + hal_pdir_t dir = d->dir == KINS_OUT ? HAL_OUT : d->dir == KINS_IO ? HAL_IO : HAL_IN; + switch (d->type) { + case KINS_PARAM_FLOAT: + res += hal_pin_new_real(comp_id, dir, &pins[i].r, d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_BIT: + res += hal_pin_new_bool(comp_id, dir, &pins[i].b, d->dflt != 0, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_S32: + res += hal_pin_new_si32(comp_id, dir, &pins[i].s, (rtapi_s32)d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_U32: + res += hal_pin_new_ui32(comp_id, dir, &pins[i].u, (rtapi_u32)d->dflt, "%s.%s", prefix, d->name); + break; + default: + res = -1; + } + } + if (res) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: pin create failed for %s\n", prefix); + return -1; + } + *out = pins; + return 0; +} // kinsParamsPinsCreate() + +void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!pins || !params || !p) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double v; + if (d->dir == KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: v = hal_get_real(pins[i].r); break; + case KINS_PARAM_BIT: v = hal_get_bool(pins[i].b) ? 1.0 : 0.0; break; + case KINS_PARAM_S32: v = hal_get_si32(pins[i].s); break; + case KINS_PARAM_U32: v = hal_get_ui32(pins[i].u); break; + default: v = 0; + } + p->geometry[i] = v; + if (d->tool) { p->tool.tran.z = v; } + } +} // kinsParamsPinsRead() + +void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s) +{ + int i; + if (!pins || !params || !s) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + if (d->dir != KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: hal_set_real(pins[i].r, s->out[i]); break; + case KINS_PARAM_BIT: hal_set_bool(pins[i].b, s->out[i] != 0); break; + case KINS_PARAM_S32: hal_set_si32(pins[i].s, (rtapi_s32)s->out[i]); break; + case KINS_PARAM_U32: hal_set_ui32(pins[i].u, (rtapi_u32)s->out[i]); break; + default: break; + } + } +} // kinsParamsPinsWrite() diff --git a/src/emc/kinematics/nonrt_kins.h b/src/emc/kinematics/nonrt_kins.h deleted file mode 100644 index ed564f21b6f..00000000000 --- a/src/emc/kinematics/nonrt_kins.h +++ /dev/null @@ -1,95 +0,0 @@ -/******************************************************************** - * 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 and a resolver callback. The module names - * each of the pins it reads, keeps the references the resolver - * returns in its own haldata, and hands back its existing forward - * and inverse. The kinematics code itself does not change. - * - * A reference does not point into the RT instance's pin. The - * resolver creates an input pin on the caller's own component and - * connects it to the signal the RT pin reads, so the reference - * belongs to the caller and rewiring cannot strand it. - * - * Name lookup belongs to the caller, userspace code linked against - * liblinuxcnchal. This file is compiled into an RT module, which - * has no business walking the HAL name space and would risk binding - * against rtlib's copy of the same symbols. - * - * Resolve input pins only. Output pins and scratch storage stay - * private to the non-RT copy, or the two copies write to each - * other's state. - * - * 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. Finds 'pin_name' in HAL, checks that it has - type 'type', and writes to 'out' a reference carrying that pin's - value. The reference is to storage the caller owns, not to the named - pin itself. 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/switchkins.c b/src/emc/kinematics/switchkins.c index d12e57fbfcb..83e2445ec40 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,10 +27,12 @@ * Using modules must supply function: switchkinsSetup() */ #include +#include #include #include #include "switchkins.h" +#include //********************************************************************* // kinematic functions (default=0 for err detection): @@ -46,6 +48,15 @@ static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL}; static KJ kjacs[SWITCHKINS_MAX_TYPES] = {NULL}; static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; +// types written as pure functions (see kinematics.h): the maths of each, +// the one RT parameter block they all read, a scratch per type, and the +// pins made from the module's table +static const kins_ops *kops[SWITCHKINS_MAX_TYPES] = {NULL}; +static kins_params rt_params; +static kins_scratch rt_scratch[SWITCHKINS_MAX_TYPES]; +static kins_pin_ref *pins; +static int inited; + // types provided, counted in rtapi_app_main() once they are all in static int kins_count; static int register_error; @@ -97,6 +108,35 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() +// the block sees the pins as they are now, and the type asked for +static void read_block(int ktype) +{ + rt_params.ktype = ktype; + kinsParamsPinsRead(pins, kp.params, kp.nparams, &rt_params); +} + +static void write_block(int ktype) +{ + kinsParamsPinsWrite(pins, kp.params, kp.nparams, &rt_scratch[ktype]); +} + +// the forward of one type, whichever way it was provided +static int call_forward(int ktype, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (kops[ktype]) { + read_block(ktype); + r = kinsOpsForward(kops[ktype], &rt_params, &rt_scratch[ktype], + joint, pos, fflags, iflags); + write_block(ktype); + return r; + } + if (!kfwds[ktype]) { return -1; } + return kfwds[ktype](joint, pos, fflags, iflags); +} + static int gui_forward_kins(const double *joints) { // the hexapod vismach gui uses these hal pins to @@ -108,14 +148,14 @@ static int gui_forward_kins(const double *joints) KINEMATICS_INVERSE_FLAGS iflags; if ( kp.gui_kinstype < 0 || kp.gui_kinstype >= kins_count - || !kfwds[kp.gui_kinstype]) { + || (!kfwds[kp.gui_kinstype] && !kops[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); + res = call_forward(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); @@ -153,6 +193,10 @@ int kinematicsSwitch(int new_switchkins_type) if (fwd_iterates[switchkins_type]) { use_lastpose[switchkins_type] = 1; // restarting a kins types } + // a pure type keeps the same restart pose in its own scratch + if (kops[switchkins_type] && kops[switchkins_type]->fwd_iterates) { + rt_scratch[switchkins_type].have_pose_seed = 1; + } return 0; // 0==> no error } // kinematicsSwitch() @@ -163,22 +207,26 @@ int kinematicsForward(const double *joint, { 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]) { + || (!kfwds[switchkins_type] && !kops[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 (kops[switchkins_type]) { + r = call_forward(switchkins_type, joint, pos, fflags, iflags); + } else { + 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; + } + 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 @@ -205,12 +253,20 @@ int kinematicsInverse(const EmcPose * pos, if ( switchkins_type < 0 || switchkins_type >= kins_count - || !kinvs[switchkins_type]) { + || (!kinvs[switchkins_type] && !kops[switchkins_type])) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Inverse BAD switchkins_type \n", switchkins_type); return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + r = kinsOpsInverse(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + pos, joint, iflags, fflags); + write_block(switchkins_type); + return r; + } r = kinvs[switchkins_type](pos, joint, iflags, fflags); return r; } // kinematicsInverse() @@ -221,9 +277,13 @@ int kinematicsToolFrame(const double *joint, { int r; - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsToolFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!ktools[switchkins_type]) { return -1; // this type does not supply one; not an error } r = ktools[switchkins_type](joint, rot, fflags); @@ -238,9 +298,13 @@ int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsWorkFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!kworks[switchkins_type]) { return -1; // this type does not supply one; not an error } // no native rotation here: the work frame has no tool axis to point the @@ -257,10 +321,12 @@ int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, int *free_directions, double *tool_spin) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type] - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + if (!kops[switchkins_type]->tool || !kops[switchkins_type]->work) { + return -1; // this type does not report its frames, so it cannot answer + } + } else if (!ktools[switchkins_type] || !kworks[switchkins_type]) { return -1; // this type does not report its frames, so it cannot answer } @@ -289,6 +355,12 @@ int kinematicsJacobian(const double *joint, if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsJacobian(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, world, jac, iflags); + } // a closed form is exact and knows its own singular poses if (kjacs[switchkins_type]) { return kjacs[switchkins_type](joint, world, jac, iflags); @@ -315,7 +387,7 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) register_error = 1; return -1; } - if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype]) { + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype] || kops[ktype]) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkinsRegister: switchkins-type %d" " already provided\n", ktype); @@ -328,6 +400,42 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) return 0; } // switchkinsRegister() +int switchkinsRegisterOps(int ktype, const kins_ops *ops) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: 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] || kops[ktype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " already provided\n", ktype); + register_error = 1; + return -1; + } + if (!ops || !ops->forward || !ops->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " has no forward or inverse\n", ktype); + register_error = 1; + return -1; + } + if (ops->tool && ops->native && !toolFrameIsProper(ops->native)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " declared a rotation that is not orthonormal with" + " determinant +1\n", ktype); + register_error = 1; + return -1; + } + kops[ktype] = ops; + return 0; +} // switchkinsRegisterOps() + int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, const PmRotationMatrix *native) { @@ -395,7 +503,46 @@ EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsRegisterJacobian); +EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); +EXPORT_SYMBOL(switchkinsDescribe); +EXPORT_SYMBOL(switchkinsDescribeSetup); + +//********************************************************************* +// the module as registered so far, described for a caller outside RT +int switchkinsDescribeSetup(const kparms *k, kins_module_info *info) +{ + int i, n = 0; + + if (!k || !info) { return -1; } + if (k->nparams < 0 || k->nparams > KINS_MAX_PARAMS + || (k->nparams > 0 && !k->params)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: %s declares a bad parameter table\n", + k->kinsname ? k->kinsname : "?"); + return -1; + } + memset(info, 0, sizeof(*info)); + info->name = k->kinsname; + info->halprefix = k->halprefix ? k->halprefix : k->kinsname; + info->params = k->params; + info->nparams = k->nparams; + info->required_coordinates = k->required_coordinates; + info->max_joints = k->max_joints; + info->allow_duplicates = k->allow_duplicates; + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + info->ops[i] = kops[i]; + if (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { n = i + 1; } + } + info->ntypes = n; + return 0; +} // switchkinsDescribeSetup() + +int switchkinsDescribe(kins_module_info *info) +{ + if (!inited) { return -1; } + return switchkinsDescribeSetup(&kp, info); +} // switchkinsDescribe() //********************************************************************* // The caller owns the hal component: it does hal_init() before this and @@ -429,7 +576,7 @@ int switchkinsInit(const int comp_id, // 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 (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { kins_count = i + 1; } } if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } @@ -456,6 +603,7 @@ int switchkinsInit(const int comp_id, // a type left out below the highest one provided is a gap, not a count for (i=0; i < kins_count; i++) { + if (kops[i]) { continue; } if (ksetups[i] && kfwds[i] && kinvs[i]) { continue; } rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: switchkins-type %d incomplete:%s%s%s\n", @@ -489,10 +637,36 @@ int switchkinsInit(const int comp_id, if (!coordinates) {coordinates = kp.required_coordinates;} + // the pure types share one block and one set of pins from the table + if (kp.params || kp.nparams) { + kins_module_info mi; + if (switchkinsDescribeSetup(&kp, &mi)) { emsg = "bad table"; goto error; } + if (kinsParamsInit(&rt_params, &mi, coordinates)) { + emsg = "coordinates"; goto error; + } + if (kinsParamsPinsCreate(comp_id, kp.halprefix, kp.params, kp.nparams, + &pins)) { + emsg = "table pin create fail"; goto error; + } + } else { + for (i=0; i < kins_count; i++) { + if (kops[i]) { + kins_module_info mi; + if (switchkinsDescribeSetup(&kp, &mi)) { emsg = "bad table"; goto error; } + if (kinsParamsInit(&rt_params, &mi, coordinates)) { + emsg = "coordinates"; goto error; + } + break; + } + } + } + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { kinsScratchInit(&rt_scratch[i]); } + for (i=0; i < kins_count; i++) { - ksetups[i](comp_id,coordinates,&kp); + if (ksetups[i]) { ksetups[i](comp_id,coordinates,&kp); } } + inited = 1; return 0; error: diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 175bfee28f7..322816b2130 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -6,8 +6,8 @@ #include "kinematics.h" -//max number of switchkins types (KS,KF,KI) a module may provide: -#define SWITCHKINS_MAX_TYPES 9 +//max number of switchkins types a module may provide: +#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES // KinematicsFORWARD functions typedef int (*KF)(const double *joint, @@ -79,10 +79,32 @@ typedef int (*KJ)(const double *joint, // otherwise the generic differences of its own inverse. extern int switchkinsRegisterJacobian(int ktype, KJ kjac); +// provide one switchkins-type written as pure functions (see kinematics.h), +// before switchkinsInit(). Its pins come from the table in kparms, shared +// by every type of the module, so it has no setup function. A type may be +// provided this way or through switchkinsRegister(), not both. +extern int switchkinsRegisterOps(int ktype, const kins_ops *ops); + // 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 ); + +// Fill kp with the defaults, run the module's switchkinsSetup() and +// register the three types it may return, so that every type goes in by +// one route. In switchkins_setup.c, which a module links only if it +// defines switchkinsSetup(); a halcompile component that registers its +// types itself does not. Returns 0 or -1. +extern int switchkinsRunSetup(kparms* kp, const char* sparm); + +// The module as the core knows it after switchkinsInit(): its table and +// the ops of every type, NULL for one provided the old way. Behind +// kinsDescribe() for the RT instance; a copy outside RT that has not been +// initialised is described by switchkins_setup.c after a replay of setup. +// Returns 0, or -1 before switchkinsInit(). +extern int switchkinsDescribe(kins_module_info *info); +extern int switchkinsDescribeSetup(const kparms *kp, kins_module_info *info); + #endif diff --git a/src/emc/kinematics/switchkins_main.c b/src/emc/kinematics/switchkins_main.c index 4a4cc05153c..8ab98b54223 100644 --- a/src/emc/kinematics/switchkins_main.c +++ b/src/emc/kinematics/switchkins_main.c @@ -19,9 +19,10 @@ /* 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. +* switchkins.c without this file and calls switchkinsInit() itself. * -* Using modules must supply function: switchkinsSetup() +* Using modules must supply function: switchkinsSetup(), which +* switchkinsRunSetup() in switchkins_setup.c runs. */ #include #include @@ -41,43 +42,8 @@ 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; - } + if (switchkinsRunSetup(&kp, sparm)) { return -1; } comp_id = hal_init(kp.kinsname); if (comp_id < 0) return comp_id; diff --git a/src/emc/kinematics/switchkins_setup.c b/src/emc/kinematics/switchkins_setup.c new file mode 100644 index 00000000000..b44b7192412 --- /dev/null +++ b/src/emc/kinematics/switchkins_setup.c @@ -0,0 +1,85 @@ +/* + License GPL Version 2 +*/ + +/* switchkins_setup.c: the part of a switchkins module that depends on the +* module supplying switchkinsSetup(). Kept apart from switchkins.c so +* that a halcompile component, which registers its types itself and has +* no switchkinsSetup(), can link the core without it. +* +* switchkinsRunSetup() is what rtapi_app_main() and EXTRA_SETUP() call +* before switchkinsInit(). kinsDescribe() is the description a copy of +* the module loaded outside RT answers with: the RT instance describes +* itself from its own state, a fresh copy replays setup first, so the +* types come out the way the module parameters decide them. +*/ +#include +#include +#include + +#include + +int switchkinsRunSetup(kparms* kp, const char* sparm) +{ + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + if (!kp) { return -1; } + memset(kp, 0, sizeof(*kp)); + + // 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 = (char*)sparm; // module parm passed to kins + + // switchkinsSetup() provides types 0,1,2 and may also call + // switchkinsRegister() or switchkinsRegisterOps() 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; + } + return 0; +} // switchkinsRunSetup() + +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + static kparms kp; + (void)coordinates; // the map is the caller's business, see kinsParamsInit() + + if (!info) { return -1; } + + // the RT instance knows itself already + if (switchkinsDescribe(info) == 0) { return 0; } + + // a copy outside RT: register the types the way the module would + if (switchkinsRunSetup(&kp, sparm)) { return -1; } + if (switchkinsDescribeSetup(&kp, info)) { return -1; } + return 0; +} // kinsDescribe() + +EXPORT_SYMBOL(switchkinsRunSetup); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 0690aa9ee39..2de2368614a 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -10,63 +10,27 @@ * ********************************************************************/ -#include #include /* RTAPI realtime OS API */ #include /* RTAPI realtime module decls */ -#include #include #include #include #include -#include "nonrt_kins.h" - - -#define SET(f) pos->f = joints[i] - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - return identityKinematicsForward(joints, pos, fflags, iflags); -} - -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ - return identityKinematicsInverse(pos, joints, iflags, fflags); -} - -int kinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsToolFrame(joints, rot, fflags); -} - -int kinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsWorkFrame(joints, rot, fflags); -} - -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) -{ - return identityKinematicsJacobian(joints, pos, jac, iflags); -} - -static KINEMATICS_TYPE ktype = -1; - -KINEMATICS_TYPE kinematicsType() -{ - return ktype; -} +#include + +// joints are axes, through whatever map coordinates= gives; the maths is +// the shared identity and the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "trivkins", + .halprefix = "trivkins", + .params = NULL, + .nparams = 0, + .required_coordinates = "", + .max_joints = EMCMOT_MAX_JOINTS, + .allow_duplicates = 1, + .ntypes = 1, + .ops = { &KINS_IDENTITY_OPS }, +}; #define TRIVKINS_DEFAULT_COORDINATES "XYZABCUVW" static char *coordinates = TRIVKINS_DEFAULT_COORDINATES; @@ -75,19 +39,40 @@ RTAPI_MP_STRING(coordinates, "Existing Axes"); static char *kinstype = "1"; // use KINEMATICS_IDENTITY RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)"); -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; +// say so when the joints are not in axis order, and which type suits that +static void show_map(KINEMATICS_TYPE ktype) +{ + kins_params p; + int a, unconventional = 0; + + if (kinsParamsInit(&p, &kins_module, coordinates)) { return; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p.joint_of_axis[a] >= 0 && p.joint_of_axis[a] != a) { unconventional = 1; } + if (p.joints_of_axis[a] & (p.joints_of_axis[a] - 1)) { unconventional = 1; } + } + if (!unconventional || !strcasecmp(coordinates, "xz")) { return; } + + rtapi_print("\ntrivkins: coordinates:%s\n", coordinates); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j; + for (j = 0; j < p.max_joints; j++) { + if (p.joints_of_axis[a] & (1 << j)) { + rtapi_print(" Joint %d ==> Axis %c\n", j, "XYZABCUVW"[a]); + } + } + } + if (ktype != KINEMATICS_BOTH) { + rtapi_print("trivkins: Recommend: kinstype=both\n"); + } + rtapi_print("\n"); +} + int rtapi_app_main(void) { - kparms ksetup; + KINEMATICS_TYPE ktype; switch (*kinstype) { case 'b': case 'B': ktype = KINEMATICS_BOTH; break; @@ -99,29 +84,14 @@ int rtapi_app_main(void) { comp_id = hal_init("trivkins"); if(comp_id < 0) return comp_id; - // see typedef for KS KinematicsSETUP: - ksetup.max_joints = EMCMOT_MAX_JOINTS; - ksetup.allow_duplicates = 1; - if (identityKinematicsSetup(comp_id, coordinates, &ksetup)) { - return -1; //setup failed + if (kinsSingleInit(comp_id, coordinates, ktype)) { + hal_exit(comp_id); + return -1; } + show_map(ktype); hal_ready(comp_id); return 0; } 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/userkfuncs.c b/src/emc/kinematics/userkfuncs.c index 81aa4c2d942..0e51ed651e5 100644 --- a/src/emc/kinematics/userkfuncs.c +++ b/src/emc/kinematics/userkfuncs.c @@ -2,6 +2,12 @@ ** switchable kinematics functions. ** License GPL Version 2 ** +** Two forms are here. USERK_OPS is the current one: identity through +** the parameter block, with no state of its own, registered by a module +** with switchkinsRegisterOps(2, &USERK_OPS). The functions below it are +** the older form, kept for the modules that still register their types +** through switchkinsSetup()'s out parameters. +** ** Example Usage (for customizing the genser-switchkins module): ** (works with rtpreempt only rtai --> Makefile needs work) ** @@ -24,6 +30,34 @@ // #include "genserkins.h" //includes gomath,hal //********************************************************************** +// the current form: pure functions of the block + +static int userk_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *world, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + // replace with the machine's own forward; the block carries the + // geometry (p->geometry[]), the joint map and the tool + return kinsIdentityForward(p, s, joint, world, fflags, iflags); +} + +static int userk_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *world, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + return kinsIdentityInverse(p, s, world, joint, iflags, fflags); +} + +const kins_ops USERK_OPS = { + .forward = userk_forward, + .inverse = userk_inverse, + // .work, .tool, .native and .jacobian are optional, see kinematics.h +}; + +//********************************************************************** +// the older form // static local variables and functions go here static int userk_inited = 0; diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 69abd527dac..b1ccec60eca 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -2,14 +2,17 @@ * Description: kinematics_user.c * Non-RT loader for kinematics modules * - * Loads a kinematics .so with dlopen and calls the nonrt_attach() it - * exports, so this process evaluates the kinematics the machine is - * running, at whatever poses it likes. See nonrt_kins.h. + * Loads a kinematics .so with dlopen, asks it to describe itself through + * kinsDescribe(), and evaluates its kinematics through the parameter + * block (see kinematics.h). The block is filled from HAL: one input pin + * of the caller's component per table entry, connected to the signal the + * RT instance's pin reads, so the values are the live ones; and the tool + * from motion's own tooloffset pins where motion is loaded, so that the + * tool the module sees is the one motion has, whether or not the config + * netted it to the module's pin. * - * Identity kinematics needs no module code: the module says so through - * nonrt_ops_t and this file maps joints to axes directly. A module - * exporting no nonrt_attach() is not an error either; the context comes - * back flagged rt_only. + * A module exporting no kinsDescribe() is not an error; the context comes + * back flagged rt_only and answers nothing. * * Author: LinuxCNC * License: GPL Version 2 @@ -19,31 +22,30 @@ ********************************************************************/ #include "kinematics_user.h" -#include #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); +typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, + kins_module_info *info); -/* One per value a kinematics module reads is a generous bound. */ -#define MAX_MADE_SIGNALS 16 -#define MAX_BOUND_PINS 16 +#define MAX_BOUND_PINS (KINS_MAX_PARAMS + AXIS_COUNT) +#define MAX_MADE_SIGNALS MAX_BOUND_PINS 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 */ + int rt_only; /* 1 if the module exports no kinsDescribe() */ KINEMATICS_TYPE kins_type; void *rt_handle; /* dlopen handle */ - nonrt_ops_t ops; + kins_module_info info; + kins_params params; + kins_scratch scratch; + int ktype; /* kinematics type being evaluated */ int num_joints; - int joint_to_axis[KINEMATICS_USER_MAX_JOINTS]; /* identity path only */ char module_name[64]; int comp_id; /* the caller's component, owns the pins made here */ const char *prefix; /* its name, which those pin names start with */ @@ -51,6 +53,10 @@ struct KinematicsUserContext { int num_made_signals; hal_refs_u *cell; /* HAL storage those pins are made against */ int num_cells; + int cell_of_param[KINS_MAX_PARAMS]; /* -1 if not bound */ + int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ + int tool_param; /* the table's tool entry, -1 if none */ + int warned_tool; }; /* ======================================================================== @@ -58,16 +64,16 @@ struct KinematicsUserContext { * ======================================================================== */ /* - * Give a kinematics module a reference to a value it asked for. + * Give the block a reference to a value it needs. * * The reference is to a pin of ours rather than into the RT instance's, - * so that its lifetime is ours: see nonrt_kins.h. Ours is connected to - * the signal the RT pin reads, or, when the RT pin has no signal, to one - * made here and removed again in kinematicsUserFree(). + * so that its lifetime is ours. Ours is connected to the signal the RT + * pin reads, or, when the RT pin has no signal, to one made here and + * removed again in kinematicsUserFree(). * * The reference has to live in HAL shared memory, since that is where * HAL rewrites it on connect and disconnect, so the pins are made - * against hal_malloc() cells and the module gets what a cell holds once + * against hal_malloc() cells and the block reads what a cell holds once * the connection is in place. */ static int make_signal(KinematicsUserContext *ctx, const char *pin_name, @@ -100,23 +106,30 @@ static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, case HAL_FLOAT: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); case HAL_S32: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); case HAL_U32: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); - case HAL_S64: return hal_pin_new_sint(comp_id, HAL_IN, &out->s, 0, "%s", name); - case HAL_U64: return hal_pin_new_uint(comp_id, HAL_IN, &out->u, 0, "%s", name); default: break; } return -1; } -static int bind_pin(const char *pin_name, hal_type_t type, - hal_refs_u *out, void *arg) +/* Does a pin of this name exist? Silent: absence is an answer, not an error. */ +static int pin_exists(const char *pin_name) +{ + hal_query_t q; + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + return hal_getref_p(&q) == 0; +} + +/* Bind pin_name; returns the cell index, or -1. */ +static int bind_pin(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type) { - KinematicsUserContext *ctx = (KinematicsUserContext *)arg; char signal[HAL_NAME_LEN + 1]; char mine[HAL_NAME_LEN + 1]; hal_refs_u *cell; hal_query_t q; - - if (!ctx || !pin_name || !out) return -1; + int idx; memset(&q, 0, sizeof(q)); q.name = pin_name; @@ -149,7 +162,8 @@ static int bind_pin(const char *pin_name, hal_type_t type, fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); return -1; } - cell = &ctx->cell[ctx->num_cells++]; + idx = ctx->num_cells; + cell = &ctx->cell[idx]; if (new_pin(ctx->comp_id, type, cell, mine) != 0) { fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); @@ -160,30 +174,103 @@ static int bind_pin(const char *pin_name, hal_type_t type, mine, signal); return -1; } + ctx->num_cells++; + return idx; +} - *out = *cell; - return 0; +static hal_type_t hal_type_of(kins_param_type t) +{ + switch (t) { + case KINS_PARAM_BIT: return HAL_BIT; + case KINS_PARAM_S32: return HAL_S32; + case KINS_PARAM_U32: return HAL_U32; + default: return HAL_FLOAT; + } } -/* ======================================================================== - * Identity joint mapping - * ======================================================================== */ +static double cell_value(const hal_refs_u *cell, kins_param_type t) +{ + switch (t) { + case KINS_PARAM_BIT: return hal_get_bool(cell->b) ? 1.0 : 0.0; + case KINS_PARAM_S32: return hal_get_si32(cell->s); + case KINS_PARAM_U32: return hal_get_ui32(cell->u); + default: return hal_get_real(cell->r); + } +} -static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coords) +/* Bind every input of the table, and motion's tool where motion is there. */ +static int bind_all(KinematicsUserContext *ctx) { - 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; + static const char letter[AXIS_COUNT] = { 'x','y','z','a','b','c','u','v','w' }; + char name[HAL_NAME_LEN + 1]; + int i; + + for (i = 0; i < KINS_MAX_PARAMS; i++) ctx->cell_of_param[i] = -1; + for (i = 0; i < AXIS_COUNT; i++) ctx->cell_of_tool[i] = -1; + ctx->tool_param = -1; + + for (i = 0; i < ctx->info.nparams; i++) { + const kins_param_desc *d = &ctx->info.params[i]; + if (d->dir == KINS_OUT) continue; + if (d->tool) ctx->tool_param = i; + snprintf(name, sizeof(name), "%s.%s", ctx->info.halprefix, d->name); + ctx->cell_of_param[i] = bind_pin(ctx, name, hal_type_of(d->type)); + if (ctx->cell_of_param[i] < 0) return -1; + } + + /* motion publishes the tool it applies; take it from there when it is + loaded, so the module sees the tool whether or not the config netted + it through. Under halrun with the module alone there is no motion, + and the module's own tool entry is all there is. */ + for (i = 0; i < AXIS_COUNT; i++) { + snprintf(name, sizeof(name), "motion.tooloffset.%c", letter[i]); + if (!pin_exists(name)) continue; + ctx->cell_of_tool[i] = bind_pin(ctx, name, HAL_FLOAT); + if (ctx->cell_of_tool[i] < 0) return -1; + } + return 0; +} + +/* The block sees the pins as they are now. */ +static void refresh(KinematicsUserContext *ctx) +{ + int i; + double tool[AXIS_COUNT]; + int have_motion_tool = 0; + + for (i = 0; i < ctx->info.nparams; i++) { + int c = ctx->cell_of_param[i]; + if (c < 0) continue; + ctx->params.geometry[i] = cell_value(&ctx->cell[c], ctx->info.params[i].type); + } + if (ctx->tool_param >= 0) { + ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; + } + + for (i = 0; i < AXIS_COUNT; i++) { + int c = ctx->cell_of_tool[i]; + tool[i] = 0.0; + if (c < 0) continue; + tool[i] = hal_get_real(ctx->cell[c].r); + have_motion_tool = 1; + } + if (!have_motion_tool) return; + + /* the module's pin and motion disagree: the config lost the tool + somewhere between them. Say so once; motion's value is the one + being cut with. */ + if (ctx->tool_param >= 0 && !ctx->warned_tool + && fabs(tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { + fprintf(stderr, + "kinematics_user: %s.%s is %.6g but motion.tooloffset.z is %.6g;" + " using motion's value\n", + ctx->info.halprefix, ctx->info.params[ctx->tool_param].name, + ctx->params.geometry[ctx->tool_param], tool[AXIS_Z]); + ctx->warned_tool = 1; + } + for (i = 0; i < AXIS_COUNT; i++) emcPoseSetAxis(&ctx->params.tool, i, tool[i]); + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = tool[AXIS_Z]; } } @@ -193,11 +280,12 @@ static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coor static int load_module(KinematicsUserContext *ctx, const char *module_name, - const char *coordinates) + const char *coordinates, + const char *sparm) { char module_path[512]; void *handle; - nonrt_attach_fn attach; + kins_describe_fn describe; snprintf(module_path, sizeof(module_path), "%s/rtlib/%s.so", EMC2_HOME, module_name); @@ -210,18 +298,18 @@ static int load_module(KinematicsUserContext *ctx, } 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); + describe = (kins_describe_fn)dlsym(handle, "kinsDescribe"); + if (!describe) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no kinsDescribe;" + " it cannot be evaluated outside RT\n", module_name); dlclose(handle); ctx->rt_handle = NULL; ctx->rt_only = 1; return -1; } - if (attach(coordinates, &ctx->ops, bind_pin, ctx) != 0) { - fprintf(stderr, "kinematicsUserInit: nonrt_attach failed for '%s'\n", + if (describe(coordinates, sparm, &ctx->info) != 0) { + fprintf(stderr, "kinematicsUserInit: kinsDescribe failed for '%s'\n", module_name); dlclose(handle); ctx->rt_handle = NULL; @@ -229,21 +317,28 @@ static int load_module(KinematicsUserContext *ctx, return -1; } - if (ctx->ops.is_identity) { - ctx->is_identity = 1; - ctx->kins_type = KINEMATICS_IDENTITY; - return 0; + if (ctx->info.ntypes < 1 || !ctx->info.ops[0]) { + fprintf(stderr, "kinematicsUserInit: '%s' has no type 0 in the" + " parameter block form\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; } - if (!ctx->ops.forward || !ctx->ops.inverse) { - fprintf(stderr, "kinematicsUserInit: '%s' set no fwd/inv\n", module_name); + if (kinsParamsInit(&ctx->params, &ctx->info, coordinates) != 0) { + fprintf(stderr, "kinematicsUserInit: '%s' refuses coordinates '%s'\n", + module_name, coordinates ? coordinates : "(default)"); dlclose(handle); ctx->rt_handle = NULL; ctx->rt_only = 1; return -1; } + kinsScratchInit(&ctx->scratch); - ctx->kins_type = KINEMATICS_BOTH; + ctx->ktype = 0; + ctx->kins_type = ctx->info.ops[0]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; return 0; } @@ -251,11 +346,12 @@ static int load_module(KinematicsUserContext *ctx, * Public API * ======================================================================== */ -KinematicsUserContext* kinematicsUserInit(const char* kins_type, - int num_joints, - const char* coordinates, - int comp_id, - const char* prefix) +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix) { KinematicsUserContext *ctx; @@ -280,59 +376,124 @@ KinematicsUserContext* kinematicsUserInit(const char* kins_type, } 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); + if (load_module(ctx, kins_type, coordinates, sparm) == 0) { + if (bind_all(ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot bind the pins of '%s'\n", + kins_type); + ctx->rt_only = 1; + } } ctx->initialized = 1; return ctx; } +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix) +{ + return kinematicsUserInitSparm(kins_type, num_joints, coordinates, NULL, + comp_id, prefix); +} + +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (ktype < 0 || ktype >= ctx->info.ntypes || !ctx->info.ops[ktype]) { + return -1; + } + ctx->ktype = ktype; + ctx->params.ktype = ktype; + kinsScratchInit(&ctx->scratch); + ctx->kins_type = ctx->info.ops[ktype]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; + return 0; +} + +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ntypes; +} + int kinematicsUserInverse(KinematicsUserContext* ctx, const EmcPose* world, double* joints) { + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + if (!ctx || !ctx->initialized || !world || !joints) return -1; + if (ctx->rt_only) 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; + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) j[i] = 0.0; + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; } - - if (ctx->rt_only) return -1; - return ctx->ops.inverse(world, joints, NULL, NULL); + for (i = 0; i < ctx->num_joints; i++) joints[i] = j[i]; + return 0; } int kinematicsUserForward(KinematicsUserContext* ctx, const double* joints, EmcPose* world) { + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + if (!ctx || !ctx->initialized || !joints || !world) return -1; + if (ctx->rt_only) 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; + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; } + memset(world, 0, sizeof(*world)); + return kinsOpsForward(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, &fflags, &iflags); +} +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double j[EMCMOT_MAX_JOINTS]; + int r, a; + + if (!ctx || !ctx->initialized || !world || !J) return -1; if (ctx->rt_only) return -1; - return ctx->ops.forward(joints, world, NULL, NULL); + + refresh(ctx); + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) j[r] = 0.0; + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; + } + if (kinsOpsJacobian(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, jac, &iflags) != 0) { + return -1; + } + for (r = 0; r < KINEMATICS_USER_MAX_JOINTS; r++) { + for (a = 0; a < AXIS_COUNT; a++) J[r][a] = jac[r][a]; + } + return 0; } int kinematicsUserIsIdentity(KinematicsUserContext* ctx) { - if (!ctx || !ctx->initialized) return 0; - return ctx->is_identity; + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ops[ctx->ktype]->identity; } int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) @@ -355,8 +516,16 @@ const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) int kinematicsUserRefreshParams(KinematicsUserContext* ctx) { - (void)ctx; - return 0; /* nothing to refresh: the bound pins are the live values */ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + refresh(ctx); + return 0; +} + +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return NULL; + refresh(ctx); + return &ctx->params; } int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index d01d8a8d277..0a7187537f9 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -6,10 +6,11 @@ * the RT kinematics interface. Used by the 9D planner to compute joint * positions from world coordinates without requiring RT kernel calls. * - * The kinematics module is loaded into this process and given input pins - * belonging to the caller's HAL component, connected to the same signals - * the running RT instance reads. Its own forward and inverse then work on - * live values, unmodified. + * The kinematics module is loaded into this process and evaluated through + * its parameter block form (see kinematics.h). The block is filled from + * input pins belonging to the caller's HAL component, connected to the + * same signals the running RT instance reads, and from motion's tool + * offset pins where motion is loaded, so the maths runs on live values. * * Author: LinuxCNC * License: GPL Version 2 @@ -62,6 +63,30 @@ KinematicsUserContext* kinematicsUserInit(const char* kins_type, int comp_id, const char* prefix); +/** + * As kinematicsUserInit(), with the module's sparm= parameter as well, for + * a module whose kinematics types depend on it (5axiskins identityfirst). + */ +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix); + +/** + * Select which kinematics type of a switchable module to evaluate. + * Type 0 is selected after init. + * + * @return 0, or -1 if the module has no such type in the block form + */ +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype); + +/** + * How many kinematics types the module has (1 for one that does not switch). + */ +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); + /** * Perform inverse kinematics (world coords -> joint positions) * @@ -86,6 +111,24 @@ int kinematicsUserForward(KinematicsUserContext* ctx, const double* joints, EmcPose* world); +/** + * The Jacobian at a pose, J[joint][axis] = d joint / d axis, from the + * module's closed form where it has one and by differencing its inverse + * where it does not. The inverse is run at the pose first, so the + * derivative is taken on the solution branch the module picks there. + * + * @return 0 on success, -1 on failure + */ +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); + +/** + * The parameter block as it stands, refreshed from HAL first. For + * reporting; the block belongs to the context. + */ +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx); + /** * Check if kinematics type is identity (world coords = joint coords) * @@ -119,20 +162,18 @@ KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); /** - * Refresh kinematics parameters (no-op) - * - * The bound pins read the live values, so there is nothing to fetch. - * This function is kept for API compatibility but does nothing. + * Copy the bound pins into the block now. Every evaluation does this + * itself; call it only to observe the values. * * @param ctx Kinematics context - * @return 0 always + * @return 0, or -1 for an RT-only context */ int kinematicsUserRefreshParams(KinematicsUserContext* ctx); /** * Check if this context is RT-only * - * An RT-only module exports no nonrt_attach() and so cannot be evaluated + * An RT-only module exports no kinsDescribe() and so cannot be evaluated * outside RT. Planner 2 is unavailable for such modules. * * @param ctx Kinematics context diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile index 553849e7ba5..3a8ccdf737a 100644 --- a/src/emc/motion_planning/Submakefile +++ b/src/emc/motion_planning/Submakefile @@ -8,9 +8,11 @@ LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ joint_limits.cc \ ) +# kins_util.c is the shared kinematics code the modules link; the loader +# needs the same block helpers and ops dispatch on this side of dlopen. LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ kinematics_user.c \ - ) + ) emc/kinematics/kins_util.c USERSRCS += $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS) diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc index a7d5a7661e7..ba8c69fdd42 100644 --- a/src/emc/motion_planning/jacobian.cc +++ b/src/emc/motion_planning/jacobian.cc @@ -12,7 +12,6 @@ #include "jacobian.hh" #include #include -#include namespace motion_planning { @@ -38,128 +37,12 @@ bool JacobianCalculator::init(KinematicsUserContext* 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); - } + std::memset(J, 0, sizeof(double) * 9 * 9); + return kinematicsUserJacobian(kins_ctx_, &pose, J) == 0; } double JacobianCalculator::conditionNumber(const double J[9][9]) { diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh index 61adca56f1f..50858536ece 100644 --- a/src/emc/motion_planning/jacobian.hh +++ b/src/emc/motion_planning/jacobian.hh @@ -3,7 +3,8 @@ * Jacobian calculation for userspace kinematics trajectory planning * * Computes the Jacobian matrix relating world velocities to joint - * velocities. For trivkins this is the identity matrix. + * velocities, from the module's own closed form through the non-RT + * kinematics loader. * * Author: LinuxCNC * License: GPL Version 2 @@ -30,8 +31,8 @@ namespace motion_planning { * 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. + * The module answers: a closed form where it has one, its inverse + * differenced where it does not. See kinematicsUserJacobian(). */ class JacobianCalculator { public: @@ -77,26 +78,9 @@ public: 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 diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 4a2b287da9d..ba13fa57886 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -123,7 +123,7 @@ static int turnKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "millturn"; diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index c1ef4b1a121..8b66b06af2d 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -222,7 +222,7 @@ static int tdrKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzab_tdr_kins"; diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 7a2b48d6659..2be983be778 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -595,7 +595,7 @@ static int toolKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzacb_trsrn"; diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index ee535daaa6e..5b58e0afa7b 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -598,7 +598,7 @@ static int toolKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzbca_trsrn"; From d62696092fc133c930addfd6babc7c4ae971b72b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:48:41 +1000 Subject: [PATCH 47/58] trtfuncs, xyzac-trt-kins, xyzbc-trt-kins, maxkins: move onto the parameter block The trt maths becomes two ops tables over one geometry table, TRT_PARAMS, with the joint map read from the block instead of the JX statics and the tool length from p->tool.tran.z, which the shared code fills from the tool-offset entry. The two modules register the tables with switchkinsRegisterOps() in the order sparm decides, and the identity and userk types come from the shared ops. The joint assignment print and the required-letter check that trtKinematicsSetup() did are now the shared code's, so the setup function goes with the haldata. maxkins keeps its fixed joint order and becomes a kins_single.c module: the table declares pivot-length as the HAL_IO pin it was and conventional-directions as before, and the three functions read the block. Pin names and defaults are unchanged throughout. --- src/Makefile | 2 + src/emc/kinematics/kinematics.h | 57 +--- src/emc/kinematics/maxkins.c | 114 ++++---- src/emc/kinematics/trtfuncs.c | 403 +++++++++++----------------- src/emc/kinematics/xyzac-trt-kins.c | 44 ++- src/emc/kinematics/xyzbc-trt-kins.c | 44 ++- 6 files changed, 257 insertions(+), 407 deletions(-) diff --git a/src/Makefile b/src/Makefile index eb6044048fc..fd4be17e059 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1145,6 +1145,8 @@ trivkins-objs += emc/kinematics/kins_single.o obj-m += maxkins.o maxkins-objs := emc/kinematics/maxkins.o +maxkins-objs += emc/kinematics/kins_util.o +maxkins-objs += emc/kinematics/kins_single.o obj-m += rotatekins.o rotatekins-objs := emc/kinematics/rotatekins.o diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 839435b90f4..9162e8e3335 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -504,6 +504,7 @@ typedef struct kins_scratch { int have_joint_seed; int iterations; int failed; + double aux[8]; /* whatever else a module carries between calls */ double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ } kins_scratch; @@ -668,56 +669,10 @@ extern int userkKinematicsInverse(const struct EmcPose * world, KINEMATICS_FORWARD_FLAGS * fflags); #endif //********************************************************************* -// xyzac,xyzbc; -extern int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - - -extern int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); +// xyzac,xyzbc (trtfuncs.c): one geometry table, the maths of each machine +extern const kins_param_desc TRT_PARAMS[]; +extern const int TRT_NPARAMS; +extern const kins_ops XYZAC_OPS; +extern const kins_ops XYZBC_OPS; //********************************************************************* diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index 28cf4edebeb..b93a76deae8 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -6,13 +6,13 @@ * * Author: Chris Radek * License: GPL Version 2 -* +* * Copyright (c) 2007 Chris Radek ********************************************************************/ /******************************************************************** -* Note: The direction of the B axis is the opposite of the -* conventional axis direction. See +* Note: The direction of the B axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -21,6 +21,7 @@ #include #include #include /* these decls */ +#include #define d2r(d) ((d)*PM_PI/180.0) #define r2d(r) ((r)*180.0/PM_PI) @@ -29,26 +30,32 @@ #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t pivot_length; - hal_bool_t conventional_directions; //default is false -} *haldata; +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc max_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IO, 0, 0.666 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0 }, // default is unconventional +}; +enum { P_PIVOT_LENGTH, P_CON }; + +#define CON(p) ((p)->geometry[P_CON] != 0 ? 1.0 : -1.0) -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int max_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; // B correction const double zb = (pivot_length + joints[8]) * cos(d2r(joints[4])); const double xb = (pivot_length + joints[8]) * sin(d2r(joints[4])); - + // C correction const double xyr = hypot(joints[0], joints[1]); const double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]); @@ -73,21 +80,23 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int max_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; // B correction const double zb = (pivot_length + pos->w) * cos(d2r(pos->b)); const double xb = (pivot_length + pos->w) * sin(d2r(pos->b)); - + // C correction const double xyr = hypot(pos->tran.x, pos->tran.y); const double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c); @@ -112,13 +121,13 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int max_jacobian(const kins_params *p, const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; const double k = M_PI/180; const double sb = sin(d2r(pos->b)), cb = cos(d2r(pos->b)); const double sc = sin(d2r(pos->c)), cc = cos(d2r(pos->c)); @@ -132,9 +141,9 @@ int kinematicsJacobian(const double *joints, for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } } - // kinematicsInverse() with the polar form expanded: rotating (x, y) - // by -c is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the - // B and U corrections are what they are written as + // max_inverse() with the polar form expanded: rotating (x, y) by -c + // is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the B and U + // corrections are what they are written as jac[0][0] = cc; jac[0][1] = sc; jac[0][4] = (con * R * cb - pos->u * sb) * k; @@ -156,39 +165,40 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops max_ops = { + .forward = max_forward, + .inverse = max_inverse, + .jacobian = max_jacobian, +}; + +// joints 0..8 are X..W in order, always; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "maxkins", + .halprefix = "maxkins", + .params = max_params, + .nparams = sizeof(max_params)/sizeof(max_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &max_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; int rtapi_app_main(void) { - int result; comp_id = hal_init("maxkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { result = -ENOMEM; goto error; } - - result = hal_pin_new_real(comp_id, HAL_IO, &(haldata->pivot_length), 0.666, "maxkins.pivot-length"); - // default is unconventional - result += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), 0, "maxkins.conventional-directions"); - - if(result < 0) goto error; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return result; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 6f77bfd92ad..a2c5f6e057f 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -25,150 +25,67 @@ * This mill has a tilting table (B axis) and horizontal rotary * mounted to the table (C axis). * -* Note: The directions of the rotational axes are the opposite of the -* conventional axis directions. See +* Note: The directions of the rotational axes are the opposite of the +* conventional axis directions. See * https://linuxcnc.org/docs/html/gcode/machining-center.html - +* +* Written as pure functions of the parameter block (see kinematics.h): +* the geometry is the table below, the joint map comes from the block, +* and the tool length is p->tool.tran.z. ********************************************************************/ #include -#include -#include -#include #include #include -static int trtfuncs_max_joints; - -// joint number assignments (-1 ==> not assigned) -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; - -struct haldata { - hal_real_t x_rot_point; - hal_real_t y_rot_point; - hal_real_t z_rot_point; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t z_offset; - hal_real_t tool_offset; - hal_bool_t conventional_directions; // default: false -} *haldata; - - -int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - int i,jno,res=0; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int rqdjoints = strlen(kp->required_coordinates); - - if (rqdjoints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: supports %d joints, <%s> requires %d\n", - kp->kinsname, - kp->max_joints, - coordinates, - rqdjoints); - goto error; - } - trtfuncs_max_joints = kp->max_joints; - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coords (order doesn't matter) - for (i=0; i < rqdjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jno < EMCMOT_MAX_JOINTS; jno++) { - if (axis_idx_for_jno[jno] == 0 && JX==-1) {JX = jno;} - if (axis_idx_for_jno[jno] == 1 && JY==-1) {JY = jno;} - if (axis_idx_for_jno[jno] == 2 && JZ==-1) {JZ = jno;} - if (axis_idx_for_jno[jno] == 3 && JA==-1) {JA = jno;} - if (axis_idx_for_jno[jno] == 4 && JB==-1) {JB = jno;} - if (axis_idx_for_jno[jno] == 5 && JC==-1) {JC = jno;} - if (axis_idx_for_jno[jno] == 6 && JU==-1) {JU = jno;} - if (axis_idx_for_jno[jno] == 7 && JV==-1) {JV = jno;} - if (axis_idx_for_jno[jno] == 8 && JW==-1) {JW = jno;} - } - - rtapi_print("%s coordinates=%s assigns:\n", kp->kinsname,coordinates); - for (jno=0; jno Axis %c\n", - jno,"XYZABCUVW"[axis_idx_for_jno[jno]]); - } - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) {goto error;} - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->x_rot_point), - 0.0, "%s.x-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->y_rot_point), - 0.0, "%s.y-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->z_rot_point), - 0.0, "%s.z-rot-point",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->z_offset), - 0.0, "%s.z-offset",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_offset), - 0.0, "%s.tool-offset",kp->halprefix); - res += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), - 0, "%s.conventional-directions", kp->halprefix); - if (res) {goto error;} - return 0; - -error: - rtapi_print_msg(RTAPI_MSG_ERR,"trtKinematicsSetup() FAIL\n"); - return -1; -} // trtKinematicsSetup() - -int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry both machines share, one pin each +const kins_param_desc TRT_PARAMS[] = { + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0.0 }, // default: false +}; +const int TRT_NPARAMS = sizeof(TRT_PARAMS)/sizeof(TRT_PARAMS[0]); + +enum { TRT_XR, TRT_YR, TRT_ZR, TRT_XO, TRT_YO, TRT_ZO, TRT_TOOL, TRT_CON }; + +// joint number assignments from the block (-1 ==> not assigned) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) + +// the direction sign the conventional-directions pin selects +#define CON(p) ((p)->geometry[TRT_CON] != 0 ? 1.0 : -1.0) + +static int xyzac_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dt = hal_get_real(haldata->tool_offset); - const double dy = hal_get_real(haldata->y_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dt = p->tool.tran.z; + const double dy = p->geometry[TRT_YO]; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = + cos(c_rad) * (joints[JX] - x_rot_point) - con * sin(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point) @@ -198,25 +115,27 @@ int xyzacKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzacKinematicsForward() +} // xyzac_forward() -int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzac_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dy = hal_get_real(haldata->y_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = pos->a*TO_RAD; const double c_rad = pos->c*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); EmcPose P; // computed position @@ -253,16 +172,12 @@ int xyzacKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); - - return 0; -} // xyzacKinematicsInverse() + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzac_inverse() -int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzac_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; // the forward transform's coefficients for a displacement of the X, Y and @@ -271,7 +186,7 @@ int xyzacKinematicsWorkFrame(const double *joints, const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad); rot->y.x = con * sin(c_rad); @@ -286,32 +201,21 @@ int xyzacKinematicsWorkFrame(const double *joints, rot->z.z = cos(a_rad); return 0; -} // xyzacKinematicsWorkFrame() - -int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzacKinematicsToolFrame() +} // xyzac_work_frame() -int xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int xyzac_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dy = hal_get_real(haldata->y_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double sa = sin(pos->a*TO_RAD), ca = cos(pos->a*TO_RAD); const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); const double X = pos->tran.x - x_rot_point; @@ -320,14 +224,14 @@ int xyzacKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a, b; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); for (a = 0; a < EMCMOT_MAX_AXIS; a++) { for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } } - // the computed position P of xyzacKinematicsInverse(), differentiated: - // its coefficients for x, y and z, and the same expressions with the + // the computed position P of xyzac_inverse(), differentiated: its + // coefficients for x, y and z, and the same expressions with the // rotation taken a quarter turn on for a and for c dP[0][0] = cc; dP[0][1] = con * sc; @@ -347,29 +251,41 @@ int xyzacKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzacKinematicsJacobian() - -int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzac_jacobian() + +// both rotaries carry the work, so the tool never turns in the machine: +// the tool frame is the shared identity one +const kins_ops XYZAC_OPS = { + .forward = xyzac_forward, + .inverse = xyzac_inverse, + .work = xyzac_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzac_jacobian, +}; + +static int xyzbc_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // Note: 'principal' joints are used - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = cos(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point) - con * sin(c_rad) * (joints[JY] - y_rot_point) @@ -398,25 +314,27 @@ int xyzbcKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzbcKinematicsForward() +} // xyzbc_forward() -int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzbc_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = pos->b*TO_RAD; const double c_rad = pos->c*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); // the offsets seen from the tilted table: the same rotation the // forward applies to them, in the same sense @@ -453,23 +371,19 @@ int xyzbcKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzbc_inverse() - return 0; -} // xyzbcKinematicsInverse() - -int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzbc_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - // see the comment in xyzacKinematicsWorkFrame() + // see the comment in xyzac_work_frame() const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad) * cos(b_rad); rot->y.x = con * sin(c_rad) * cos(b_rad); @@ -484,32 +398,21 @@ int xyzbcKinematicsWorkFrame(const double *joints, rot->z.z = cos(b_rad); return 0; -} // xyzbcKinematicsWorkFrame() - -int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzbcKinematicsToolFrame() +} // xyzbc_work_frame() -int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int xyzbc_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double sb = sin(pos->b*TO_RAD), cb = cos(pos->b*TO_RAD); const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); const double X = pos->tran.x - x_rot_point; @@ -518,14 +421,14 @@ int xyzbcKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a, b; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); for (a = 0; a < EMCMOT_MAX_AXIS; a++) { for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } } - // see the comment in xyzacKinematicsJacobian(); dpx and dpz of the - // inverse depend on b as well + // see the comment in xyzac_jacobian(); dpx and dpz of the inverse + // depend on b as well dP[0][0] = cc * cb; dP[0][1] = con * sc * cb; dP[0][2] = - con * sb; @@ -544,7 +447,15 @@ int xyzbcKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzbcKinematicsJacobian() + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzbc_jacobian() + +const kins_ops XYZBC_OPS = { + .forward = xyzbc_forward, + .inverse = xyzbc_inverse, + .work = xyzbc_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzbc_jacobian, +}; diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 666d0ba128f..3fc14fc9de4 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -4,11 +4,12 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ #include @@ -23,43 +24,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzac-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzac-trt-kins"; // hal pin names kp->required_coordinates = "xyzac"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzacKinematicsForward; - *kinv1 = xyzacKinematicsInverse; - switchkinsRegisterFrames(1, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(1, xyzacKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZAC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzacKinematicsForward; - *kinv0 = xyzacKinematicsInverse; - switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(0, xyzacKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; + switchkinsRegisterOps(0, &XYZAC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 9ac7ed9e3c0..45c41b448dd 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -4,11 +4,12 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ #include @@ -23,43 +24,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzbc-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzbc-trt-kins"; // hal pin names kp->required_coordinates = "xyzbc"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzbcKinematicsForward; - *kinv1 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(1, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(1, xyzbcKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZBC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzbcKinematicsForward; - *kinv0 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(0, xyzbcKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; + switchkinsRegisterOps(0, &XYZBC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } From e26375d74e0055e7cae46ab98009a1f3d16b2ce8 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:57:51 +1000 Subject: [PATCH 48/58] corexykins, rotatekins, rosekins, tripodkins, scorbot-kins, the deltas, matrixkins, userkins: move onto the parameter block Each becomes a kins_module description over one ops table and links kins_single.c for the classic entry points. The maths is unchanged; where it read a pin it reads the block, and what it kept between calls it keeps in the scratch: rosekins counts its turns in scratch aux and reports revolutions, theta_degrees and bigtheta_degrees as declared outputs, so each caller counts its own. tripodkins keeps Bx, Cx and Cy as the HAL_IO pins they were. kinematicsHome() goes from corexykins and rotatekins; nothing called it. The two delta modules share their maths with a python module through a common header whose geometry was a set of statics filled by set_geometry(). The geometry is a struct the caller passes now, so the realtime module fills one from the block and the python module keeps its own; the python API is unchanged. matrixkins declared its nine coefficients as HAL parameters; the table makes them pins of the same names, which setp sets the same way. userkins, the template for kinematics built out of tree, includes kins_util.c and kins_single.c by name so halcompile builds it on its own; kins_single.c joins the sources installed in share/linuxcnc, and the in-tree build resolves the same names from emc/kinematics. Its example pins become a declared input and output. --- .gitignore | 1 + debian/linuxcnc-uspace-dev.install | 1 + src/Makefile | 19 +- src/emc/kinematics/Submakefile | 3 +- src/emc/kinematics/corexykins.c | 72 +++--- src/emc/kinematics/lineardeltakins-common.h | 46 ++-- src/emc/kinematics/lineardeltakins.c | 94 ++++---- src/emc/kinematics/lineardeltakins.cc | 14 +- src/emc/kinematics/rosekins.c | 110 +++++---- src/emc/kinematics/rotarydeltakins-common.h | 62 +++-- src/emc/kinematics/rotarydeltakins.c | 116 ++++----- src/emc/kinematics/rotarydeltakins.cc | 15 +- src/emc/kinematics/rotatekins.c | 84 ++++--- src/emc/kinematics/scorbot-kins.c | 127 +++------- src/emc/kinematics/tripodkins.c | 246 +++++--------------- src/hal/components/Submakefile | 1 + src/hal/components/matrixkins.comp | 196 ++++++++-------- src/hal/components/userkins.comp | 174 +++++++------- 18 files changed, 650 insertions(+), 731 deletions(-) diff --git a/.gitignore b/.gitignore index 18eb2868130..19647ebbccd 100644 --- a/.gitignore +++ b/.gitignore @@ -23,6 +23,7 @@ share/desktop-directories/linuxcnc-doc.directory share/linuxcnc/mesa_modbus.c.tmpl share/linuxcnc/switchkins.c share/linuxcnc/kins_util.c +share/linuxcnc/kins_single.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 39c124d3532..251c401a9e9 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -7,3 +7,4 @@ usr/share/linuxcnc/Makefile.modinc usr/share/linuxcnc/mesa_modbus.c.tmpl usr/share/linuxcnc/switchkins.c usr/share/linuxcnc/kins_util.c +usr/share/linuxcnc/kins_single.c diff --git a/src/Makefile b/src/Makefile index fd4be17e059..ed4fd67bf88 100644 --- a/src/Makefile +++ b/src/Makefile @@ -777,7 +777,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/ + $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c ../src/emc/kinematics/kins_single.c $(DESTDIR)$(prefix)/share/linuxcnc/ install-kernel-indep: install-python install-python: install-dirs @@ -907,6 +907,9 @@ endif # "kbuild" system. $(BASEPWD) is used here, instead of relative paths, because # that's what kbuild seems to require +# A component built in tree includes the shared kinematics sources by the +# bare names the out-of-tree build resolves in share/linuxcnc +RTFLAGS += -I$(BASEPWD)/emc/kinematics EXTRA_CFLAGS := $(filter-out -ffast-math,$(RTFLAGS)) -D__MODULE__ \ -I$(BASEPWD)/../include -I$(BASEPWD) \ -DSEQUENTIAL_SUPPORT -DHAL_SUPPORT -DDYNAMIC_PLCSIZE -DRT_SUPPORT -DOLD_TIMERS_MONOS_SUPPORT -DMODBUS_IO_MASTER \ @@ -1150,15 +1153,23 @@ maxkins-objs += emc/kinematics/kins_single.o obj-m += rotatekins.o rotatekins-objs := emc/kinematics/rotatekins.o +rotatekins-objs += emc/kinematics/kins_util.o +rotatekins-objs += emc/kinematics/kins_single.o obj-m += tripodkins.o tripodkins-objs := emc/kinematics/tripodkins.o +tripodkins-objs += emc/kinematics/kins_util.o +tripodkins-objs += emc/kinematics/kins_single.o obj-m += corexykins.o corexykins-objs := emc/kinematics/corexykins.o +corexykins-objs += emc/kinematics/kins_util.o +corexykins-objs += emc/kinematics/kins_single.o obj-m += lineardeltakins.o lineardeltakins-objs := emc/kinematics/lineardeltakins.o +lineardeltakins-objs += emc/kinematics/kins_util.o +lineardeltakins-objs += emc/kinematics/kins_single.o obj-m += pentakins.o pentakins-objs := emc/kinematics/pentakins.o @@ -1167,14 +1178,20 @@ pentakins-objs += $(MATHSTUB) obj-m += rotarydeltakins.o rotarydeltakins-objs := emc/kinematics/rotarydeltakins.o +rotarydeltakins-objs += emc/kinematics/kins_util.o +rotarydeltakins-objs += emc/kinematics/kins_single.o rotarydeltakins-objs += libposemath/_posemath.o rotarydeltakins-objs += $(MATHSTUB) obj-m += rosekins.o rosekins-objs := emc/kinematics/rosekins.o +rosekins-objs += emc/kinematics/kins_util.o +rosekins-objs += emc/kinematics/kins_single.o obj-m += scorbot-kins.o scorbot-kins-objs := emc/kinematics/scorbot-kins.o +scorbot-kins-objs += emc/kinematics/kins_util.o +scorbot-kins-objs += emc/kinematics/kins_single.o ifeq ($(origin userkfuncs), undefined) # use template: diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 7e2f2d84b4b..c71c18696e2 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -39,7 +39,8 @@ PYTARGETS += $(RDELTAMODULE) # in-tree ones link it. EMCKINEMATICSSRCS = \ ../share/linuxcnc/switchkins.c \ - ../share/linuxcnc/kins_util.c + ../share/linuxcnc/kins_util.c \ + ../share/linuxcnc/kins_single.c $(EMCKINEMATICSSRCS): ../share/linuxcnc/%.c: ./emc/kinematics/%.c $(ECHO) Copying switchkins source $(notdir $@) diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index 473a2ceede1..353ae0f5a84 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -8,12 +8,15 @@ #include #include #include +#include -int kinematicsForward(const double *joints - ,EmcPose *pos - ,const KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { +static int corexy_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)p; + (void)s; (void)fflags; (void)iflags; pos->tran.x = 0.5 * (joints[0] + joints[1]); @@ -29,11 +32,13 @@ int kinematicsForward(const double *joints return 0; } -int kinematicsInverse(const EmcPose *pos - ,double *joints - ,const KINEMATICS_INVERSE_FLAGS *iflags - ,KINEMATICS_FORWARD_FLAGS *fflags - ) { +static int corexy_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)s; (void)iflags; (void)fflags; joints[0] = pos->tran.x + pos->tran.y; @@ -49,12 +54,13 @@ int kinematicsInverse(const EmcPose *pos return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int corexy_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { int j, a; + (void)p; (void)joints; (void)pos; (void)iflags; @@ -68,23 +74,26 @@ int kinematicsJacobian(const double *joints, return 0; } -int kinematicsHome(EmcPose *world - ,double *joint - ,KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { - *fflags = 0; - *iflags = 0; - return kinematicsForward(joint, world, fflags, iflags); -} +static const kins_ops corexy_ops = { + .forward = corexy_forward, + .inverse = corexy_inverse, + .jacobian = corexy_jacobian, +}; -KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } +// no geometry: the belts are what they are. Joints 0..8 are the nine +// letters in order; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "corexykins", + .halprefix = "corexykins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &corexy_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -92,6 +101,11 @@ int rtapi_app_main(void) { comp_id = hal_init("corexykins"); if(comp_id < 0) return comp_id; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); return 0; } diff --git a/src/emc/kinematics/lineardeltakins-common.h b/src/emc/kinematics/lineardeltakins-common.h index 6e0b0037375..203343a20e1 100644 --- a/src/emc/kinematics/lineardeltakins-common.h +++ b/src/emc/kinematics/lineardeltakins-common.h @@ -32,10 +32,16 @@ // common routines used by the userspace kinematics and the realtime kinematics // user must include a math.h-type header first // Inspired by Marlin delta firmware and https://gist.github.com/kastner/5279172 +// +// The geometry is a value the caller holds and passes in, so the same +// routines serve the realtime module through its parameter block and the +// python module through its own copy. #include -static double L, R; -static double Ax, Ay, Bx, By, Cx, Cy, L2; +typedef struct { + double L, R; + double Ax, Ay, Bx, By, Cx, Cy, L2; +} lineardelta_geometry; #define SQ3 (sqrt(3)) @@ -44,31 +50,30 @@ static double Ax, Ay, Bx, By, Cx, Cy, L2; static double sq(double x) { return x*x; } -static void set_geometry(double r_, double l_) +static void lineardelta_set_geometry(lineardelta_geometry *g, double r_, double l_) { - if(L == l_ && R == r_) return; - - L = l_; - R = r_; + g->L = l_; + g->R = r_; - L2 = sq(L); + g->L2 = sq(g->L); - Ax = 0.0; - Ay = R; + g->Ax = 0.0; + g->Ay = g->R; - Bx = -SIN_60 * R; - By = -COS_60 * R; + g->Bx = -SIN_60 * g->R; + g->By = -COS_60 * g->R; - Cx = SIN_60 * R; - Cy = -COS_60 * R; + g->Cx = SIN_60 * g->R; + g->Cy = -COS_60 * g->R; } -static int kinematics_inverse(const EmcPose *pos, double *joints) +static int lineardelta_inverse(const lineardelta_geometry *g, + const EmcPose *pos, double *joints) { double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; - joints[0] = z + sqrt(L2 - sq(Ax-x) - sq(Ay-y)); - joints[1] = z + sqrt(L2 - sq(Bx-x) - sq(By-y)); - joints[2] = z + sqrt(L2 - sq(Cx-x) - sq(Cy-y)); + joints[0] = z + sqrt(g->L2 - sq(g->Ax-x) - sq(g->Ay-y)); + joints[1] = z + sqrt(g->L2 - sq(g->Bx-x) - sq(g->By-y)); + joints[2] = z + sqrt(g->L2 - sq(g->Cx-x) - sq(g->Cy-y)); joints[3] = pos->a; joints[4] = pos->b; joints[5] = pos->c; @@ -80,11 +85,14 @@ static int kinematics_inverse(const EmcPose *pos, double *joints) ? -1 : 0; } -static int kinematics_forward(const double *joints, EmcPose *pos) +static int lineardelta_forward(const lineardelta_geometry *g, + const double *joints, EmcPose *pos) { double q1 = joints[0]; double q2 = joints[1]; double q3 = joints[2]; + const double Ay = g->Ay, Bx = g->Bx, By = g->By, Cx = g->Cx, Cy = g->Cy; + const double L = g->L; double den = (By-Ay)*Cx-(Cy-Ay)*Bx; diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 541643fef74..8aa454258d8 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -18,52 +18,67 @@ #include #include #include +#include #include "lineardeltakins-common.h" -static struct haldata -{ - hal_real_t r; - hal_real_t l; -} *haldata; +// the two lengths, one pin each +static const kins_param_desc ld_params[] = { + { "R", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_RADIUS }, + { "L", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_DIAGONAL_ROD }, +}; +enum { P_R, P_L }; static int comp_id; -int kinematicsForward(const double * joints, +// the tower positions follow from the block's two lengths +static void geometry_of(const kins_params *p, lineardelta_geometry *g) +{ + lineardelta_set_geometry(g, p->geometry[P_R], p->geometry[P_L]); +} + +static int ld_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + lineardelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return lineardelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int ld_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + lineardelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return lineardelta_inverse(&g, pos, joints); } -int kinematicsJacobian(const double *joints, +static int ld_jacobian(const kins_params *p, const double *joints, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags) { + lineardelta_geometry g; double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; int i, j, a; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); + geometry_of(p, &g); for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } } // each carriage is the platform height plus the rise of its rod, and // the rise changes with the horizontal offset from the tower for (i = 0; i < 3; i++) { - double tx = (i == 0) ? Ax : (i == 1) ? Bx : Cx; - double ty = (i == 0) ? Ay : (i == 1) ? By : Cy; + double tx = (i == 0) ? g.Ax : (i == 1) ? g.Bx : g.Cx; + double ty = (i == 0) ? g.Ay : (i == 1) ? g.By : g.Cy; double rise = joints[i] - z; if (rise <= 0) { return -1; } jac[i][0] = (tx - x)/rise; @@ -74,32 +89,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops ld_ops = { + .forward = ld_forward, + .inverse = ld_inverse, + .jacobian = ld_jacobian, +}; + +// three towers for the three linear coordinates, the rest passed +// through; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "lineardeltakins", + .halprefix = "lineardeltakins", + .params = ld_params, + .nparams = sizeof(ld_params)/sizeof(ld_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &ld_ops }, +}; int rtapi_app_main(void) { - int retval; - comp_id = hal_init("lineardeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->r, DELTA_RADIUS, "lineardeltakins.R")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->l, DELTA_DIAGONAL_ROD, "lineardeltakins.L")) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -107,9 +128,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/lineardeltakins.cc b/src/emc/kinematics/lineardeltakins.cc index 351746081c7..780542ba380 100644 --- a/src/emc/kinematics/lineardeltakins.cc +++ b/src/emc/kinematics/lineardeltakins.cc @@ -21,11 +21,19 @@ using namespace boost::python; #define isnan(x) std::isnan(x) #include "lineardeltakins-common.h" +// the python module keeps one geometry, set from python +static lineardelta_geometry geometry; + +static void set_geometry(double r, double l) +{ + lineardelta_set_geometry(&geometry, r, l); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = lineardelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -35,7 +43,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = lineardelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -43,7 +51,7 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(R, L); + return make_tuple(geometry.R, geometry.L); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index 9f73fbc3f9d..1622542154f 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -21,29 +21,36 @@ #include #include #include +#include -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); #ifndef hypot #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t revolutions; - hal_real_t theta_degrees; - hal_real_t bigtheta_degrees; -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the inverse reports the turn count it keeps and the angles it saw +static const kins_param_desc rose_params[] = { + { "revolutions", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "theta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "bigtheta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, +}; +enum { O_REVOLUTIONS, O_THETA, O_BIGTHETA }; + +// what the inverse carries from one call to the next: the quadrant it +// last saw and the turns it has counted. In the scratch, so that each +// caller counts its own. +#define OLDQUAD(s) ((s)->aux[0]) +#define REVOLUTIONS(s) ((s)->aux[1]) + +static int rose_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double radius,z,theta; @@ -65,18 +72,20 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rose_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)iflags; (void)fflags; // There is a potential problem when accumulating bigtheta -- loss of // precision based on size of mantissa -- but in practice, it is probably ok - static int oldquad; - static int revolutions; + int oldquad = (int)OLDQUAD(s); + int revolutions = (int)REVOLUTIONS(s); double theta,bigtheta; int nowquad = 0; @@ -95,9 +104,9 @@ int kinematicsInverse(const EmcPose * pos, theta = atan2(y,x); bigtheta = theta + PM_2_PI * revolutions; - hal_set_real(haldata->revolutions, revolutions); - hal_set_real(haldata->theta_degrees, theta * TO_DEG); - hal_set_real(haldata->bigtheta_degrees, bigtheta * TO_DEG); + s->out[O_REVOLUTIONS] = revolutions; + s->out[O_THETA] = theta * TO_DEG; + s->out[O_BIGTHETA] = bigtheta * TO_DEG; joints[0] = hypot(x,y); joints[1] = z; @@ -109,19 +118,21 @@ int kinematicsInverse(const EmcPose * pos, joints[7] = 0; joints[8] = 0; - oldquad = nowquad; + OLDQUAD(s) = nowquad; + REVOLUTIONS(s) = revolutions; return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int rose_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { double x = pos->tran.x, y = pos->tran.y; double r2 = x*x + y*y; double r = sqrt(r2); int j, a; + (void)p; (void)joints; (void)iflags; // on the axis the angle is undefined and its rate unbounded @@ -136,34 +147,39 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops rose_ops = { + .forward = rose_forward, + .inverse = rose_inverse, + .jacobian = rose_jacobian, +}; + +// joints 0..2 are radius, z and the unwrapped angle; the entry points +// come from kins_single.c +const kins_module_info kins_module = { + .name = "rosekins", + .halprefix = "rosekins", + .params = rose_params, + .nparams = sizeof(rose_params)/sizeof(rose_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rose_ops }, +}; static int comp_id; void rtapi_app_exit(void) { hal_exit(comp_id); } int rtapi_app_main(void) { - int ans; comp_id = hal_init("rosekins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { ans = -ENOMEM; goto error; } - - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->revolutions), 0.0, "rosekins.revolutions")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->theta_degrees), 0.0, "rosekins.theta_degrees")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->bigtheta_degrees), 0.0, "rosekins.bigtheta_degrees")) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return ans; } diff --git a/src/emc/kinematics/rotarydeltakins-common.h b/src/emc/kinematics/rotarydeltakins-common.h index 59cc872200c..95c6ce95c9e 100644 --- a/src/emc/kinematics/rotarydeltakins-common.h +++ b/src/emc/kinematics/rotarydeltakins-common.h @@ -40,6 +40,10 @@ positive, the Z coordinate will get more negative. Joint zero is the one whose thigh swings in the YZ plane. + + The geometry is a value the caller holds and passes in, so the same + routines serve the realtime module through its parameter block and the + python module through its own copy. */ #ifndef LINUXCNCROTARYDELTAKINS_COMMON_H @@ -47,17 +51,19 @@ #include -// distance from origin to a hip joint -static double platformradius; +typedef struct { + // distance from origin to a hip joint + double platformradius; -// thigh connects the hip to the knee -static double thighlength; + // thigh connects the hip to the knee + double thighlength; -// shin (the parallelogram) connects the knee to the foot -static double shinlength; + // shin (the parallelogram) connects the knee to the foot + double shinlength; -// distance from center of foot (controlled point) to an ankle joint -static double footradius; + // distance from center of foot (controlled point) to an ankle joint + double footradius; +} rotarydelta_geometry; #ifndef sq #define sq(a) ((a)*(a)) @@ -66,15 +72,21 @@ static double footradius; #define D2R(d) ((d)*M_PI/180.) #endif -static void set_geometry(double pfr, double tl, double sl, double fr) { - platformradius = pfr; - thighlength = tl; - shinlength = sl; - footradius = fr; +static void rotarydelta_set_geometry(rotarydelta_geometry *g, + double pfr, double tl, double sl, double fr) { + g->platformradius = pfr; + g->thighlength = tl; + g->shinlength = sl; + g->footradius = fr; } // Given three hip joint angles, find the controlled point -static int kinematics_forward(const double *joints, EmcPose *pos) { +static int rotarydelta_forward(const rotarydelta_geometry *g, + const double *joints, EmcPose *pos) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double j0 = joints[0], j1 = joints[1], @@ -139,7 +151,12 @@ static int kinematics_forward(const double *joints, EmcPose *pos) { // Given controlled point, find joint zero's angle // (J0 is the easy one in the ZY plane) -static int inverse_j0(double x, double y, double z, double *theta) { +static int rotarydelta_inverse_j0(const rotarydelta_geometry *g, + double x, double y, double z, double *theta) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double a, b, d, knee_y, knee_z; a = 0.5 * (sq(x) + sq(y - footradius) + sq(z) + sq(thighlength) - @@ -157,25 +174,26 @@ static int inverse_j0(double x, double y, double z, double *theta) { return 0; } -static void rotate(double *x, double *y, double theta) { +static void rotarydelta_rotate(double *x, double *y, double theta) { double xx, yy; xx = *x, yy = *y; *x = xx * cos(theta) - yy * sin(theta); *y = xx * sin(theta) + yy * cos(theta); } -static int kinematics_inverse(const EmcPose *pos, double *joints) { +static int rotarydelta_inverse(const rotarydelta_geometry *g, + const EmcPose *pos, double *joints) { double xr, yr; - if(inverse_j0(pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; + if(rotarydelta_inverse_j0(g, pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; // now use symmetry property to get the other two just as easily... xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, -2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[1])) return -1; + rotarydelta_rotate(&xr, &yr, -2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[1])) return -1; xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, 2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[2])) return -1; + rotarydelta_rotate(&xr, &yr, 2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[2])) return -1; joints[3] = pos->a; joints[4] = pos->b; diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index a2f52c10c1c..4cee9c38173 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -19,75 +19,90 @@ #include #include #include +#include #include "rotarydeltakins-common.h" -static struct haldata -{ - hal_real_t pfr; - hal_real_t tl; - hal_real_t sl; - hal_real_t fr; -} *haldata; +// the four lengths, one pin each +static const kins_param_desc rd_params[] = { + { "platformradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_PFR }, + { "thighlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_TL }, + { "shinlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_SL }, + { "footradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_FR }, +}; +enum { P_PFR, P_TL, P_SL, P_FR }; static int comp_id; -int kinematicsForward(const double * joints, +static void geometry_of(const kins_params *p, rotarydelta_geometry *g) +{ + rotarydelta_set_geometry(g, p->geometry[P_PFR], p->geometry[P_TL], + p->geometry[P_SL], p->geometry[P_FR]); +} + +static int rd_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + rotarydelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return rotarydelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int rd_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + rotarydelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return rotarydelta_inverse(&g, pos, joints); } -int kinematicsJacobian(const double *joints, +static int rd_jacobian(const kins_params *p, const double *joints, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags) { + rotarydelta_geometry g; int i, j, a; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); + geometry_of(p, &g); for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } } // The foot stays a shin length from each knee, so along a leg the // motion of the foot and the motion of the knee agree: // (P - K) . dP = (P - K) . dK/dq dq - // K is the knee less the foot offset, written as kinematics_forward() + // K is the knee less the foot offset, written as rotarydelta_forward() // writes it, and q the hip angle that swings it. for (i = 0; i < 3; i++) { double q = D2R(joints[i]); - double reach = platformradius - footradius + thighlength * cos(q); + double reach = g.platformradius - g.footradius + g.thighlength * cos(q); double kx, ky, kz, dkx, dky, dkz, px, py, pz, denom; switch (i) { case 0: kx = 0; ky = -reach; - dkx = 0; dky = thighlength * sin(q); + dkx = 0; dky = g.thighlength * sin(q); break; case 1: kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; - dkx = -thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 0.5; + dkx = -g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; break; default: kx = -reach * 0.5 * sqrt(3); ky = reach * 0.5; - dkx = thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 0.5; + dkx = g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; break; } - kz = -thighlength * sin(q); - dkz = -thighlength * cos(q); + kz = -g.thighlength * sin(q); + dkz = -g.thighlength * cos(q); px = pos->tran.x - kx; py = pos->tran.y - ky; pz = pos->tran.z - kz; @@ -103,36 +118,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops rd_ops = { + .forward = rd_forward, + .inverse = rd_inverse, + .jacobian = rd_jacobian, +}; + +// three hips for the three linear coordinates, the rest passed through; +// the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotarydeltakins", + .halprefix = "rotarydeltakins", + .params = rd_params, + .nparams = sizeof(rd_params)/sizeof(rd_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rd_ops }, +}; int rtapi_app_main(void) { - int retval; - comp_id = hal_init("rotarydeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->pfr, RDELTA_PFR, "rotarydeltakins.platformradius")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->tl, RDELTA_TL, "rotarydeltakins.thighlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->sl, RDELTA_SL, "rotarydeltakins.shinlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->fr, RDELTA_FR, "rotarydeltakins.footradius")) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -140,9 +157,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.cc b/src/emc/kinematics/rotarydeltakins.cc index 49a26b3153e..a8227573ab9 100644 --- a/src/emc/kinematics/rotarydeltakins.cc +++ b/src/emc/kinematics/rotarydeltakins.cc @@ -20,11 +20,19 @@ #include using namespace boost::python; +// the python module keeps one geometry, set from python +static rotarydelta_geometry geometry; + +static void set_geometry(double pfr, double tl, double sl, double fr) +{ + rotarydelta_set_geometry(&geometry, pfr, tl, sl, fr); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = rotarydelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -34,7 +42,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = rotarydelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -42,7 +50,8 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(platformradius, thighlength, shinlength, footradius); + return make_tuple(geometry.platformradius, geometry.thighlength, + geometry.shinlength, geometry.footradius); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index b5b648b4b38..6fe38d8c11a 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -7,7 +7,7 @@ * Author: Chris Radek * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2006 All rights reserved. * ********************************************************************/ @@ -17,12 +17,16 @@ #include #include #include /* these decls */ +#include -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int rotate_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double c_rad = -joints[5]*M_PI/180; @@ -39,11 +43,14 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rotate_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (void)iflags; (void)fflags; double c_rad = pos->c*M_PI/180; @@ -60,14 +67,15 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int rotate_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { double c_rad = pos->c*M_PI/180; double cc = cos(c_rad), sc = sin(c_rad); int j, a; + (void)p; (void)joints; (void)iflags; for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { @@ -83,38 +91,40 @@ int kinematicsJacobian(const double *joints, return 0; } -/* implemented for these kinematics as giving joints preference */ -int kinematicsHome(EmcPose * world, - double *joint, - KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - *fflags = 0; - *iflags = 0; +static const kins_ops rotate_ops = { + .forward = rotate_forward, + .inverse = rotate_inverse, + .jacobian = rotate_jacobian, +}; - return kinematicsForward(joint, world, fflags, iflags); -} +// no geometry; joints 0..8 are the nine letters in order, and the entry +// points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotatekins", + .halprefix = "rotatekins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rotate_ops }, +}; -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); int comp_id; int rtapi_app_main(void) { comp_id = hal_init("rotatekins"); - if(comp_id > 0) { - hal_ready(comp_id); - return 0; + if(comp_id < 0) return comp_id; + + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - return comp_id; + + hal_ready(comp_id); + return 0; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index b7f933a3b71..43e96638389 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -43,6 +43,7 @@ #include #include #include +#include // @@ -75,12 +76,15 @@ static void compute_j1_cartesian_location(double j0, EmcPose *j1_cart) { // Forward kinematics takes the joint positions and computes the cartesian // coordinates of the controlled point. -int kinematicsForward( +static int scorbot_forward( + const kins_params *p, kins_scratch *s, const double *joints, EmcPose *pose, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags ) { + (void)p; + (void)s; (void)fflags; (void)iflags; EmcPose j1_vector; // the vector from j0 ("base") to joint 1 ("shoulder", end of link 0) @@ -89,16 +93,13 @@ int kinematicsForward( double r; - // rtapi_print("fwd: j0=%f, j1=%f, j2=%f\n", joints[0], joints[1], joints[2]); compute_j1_cartesian_location(joints[0], &j1_vector); - // rtapi_print("fwd: j1=(%f, %f, %f)\n", j1_vector.tran.x, j1_vector.tran.y, j1_vector.tran.z); // Link 1 connects j1 (shoulder) to j2 (elbow). r = L1_LENGTH * cos(TO_RAD * joints[1]); j2_vector.tran.x = r * cos(TO_RAD * joints[0]); j2_vector.tran.y = r * sin(TO_RAD * joints[0]); j2_vector.tran.z = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("fwd: j2=(%f, %f, %f)\n", j2_vector.tran.x, j2_vector.tran.y, j2_vector.tran.z); // Link 2 connects j2 (elbow) to j3 (wrist). // J3 is the controlled point. @@ -106,13 +107,11 @@ int kinematicsForward( j3_vector.tran.x = r * cos(TO_RAD * joints[0]); j3_vector.tran.y = r * sin(TO_RAD * joints[0]); j3_vector.tran.z = L2_LENGTH * sin(TO_RAD * joints[2]); - // rtapi_print("fwd: j3=(%f, %f, %f)\n", j3_vector.tran.x, j3_vector.tran.y, j3_vector.tran.z); // The end-effector location is the sum of the linkage vectors. pose->tran.x = j1_vector.tran.x + j2_vector.tran.x + j3_vector.tran.x; pose->tran.y = j1_vector.tran.y + j2_vector.tran.y + j3_vector.tran.y; pose->tran.z = j1_vector.tran.z + j2_vector.tran.z + j3_vector.tran.z; - // rtapi_print("fwd: pose=(%f, %f, %f)\n", pose->tran.x, pose->tran.y, pose->tran.z); // A and B are wrist roll and pitch, handled in hal by external kinematics pose->a = joints[3]; @@ -134,15 +133,17 @@ int kinematicsForward( // is the horizontal distance (ie, in the XY plane) of the controlled // point from J0. // -int kinematicsInverse( +static int scorbot_inverse( + const kins_params *p, kins_scratch *s, const EmcPose *pose, double *joints, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags ) { + (void)p; + (void)s; (void)iflags; (void)fflags; - // EmcPose j1_cart; double distance_to_cp, distance_to_center; double r_j1, z_j1; // (r_j1, z_j1) is the location of J1 in the RZ plane double r_cp, z_cp; // (r_cp, z_cp) is the location of the controlled point in the RZ plane @@ -152,16 +153,10 @@ int kinematicsInverse( // the location of J2, this is what we're trying to find double z_j2; - // rtapi_print("inv: x=%f, y=%f, z=%f\n", pose->tran.x, pose->tran.y, pose->tran.z); - // J0 is easy. Project the (X, Y, Z) of the pose onto the Z=0 plane. // J0 points at the projected (X, Y) point. tan(J0) = Y/X // J0 then defines the plane that the rest of the arm operates in. joints[0] = TO_DEG * atan2(pose->tran.y, pose->tran.x); - // rtapi_print("inv: j0=%f\n", joints[0]); - - // compute_j1_cartesian_location(joints[0], &j1_cart); - // rtapi_print("inv: j1=(X=%f, Y=%f, Z=%f)\n", j1_cart.tran.x, j1_cart.tran.y, j1_cart.tran.z); // FIXME: Until i figure the wrist differential out, the controlled // point will be the location of the wrist joint, J3/J4. @@ -175,19 +170,16 @@ int kinematicsInverse( // of J0. This is just a known, static vector. r_j1 = L0_HORIZONTAL_DISTANCE; z_j1 = L0_VERTICAL_DISTANCE; - // rtapi_print("inv: r_j1=%f, z_j1=%f\n", r_j1, z_j1); // (r_cp, z_cp) is the location of J3 (the controlled point), again in // the plane defined by the angle of J0, with the origin of the // machine. r_cp = sqrt(pow(pose->tran.x, 2) + pow(pose->tran.y, 2)); z_cp = pose->tran.z; - // rtapi_print("inv: r_cp=%f, z_cp=%f (controlled point)\n", r_cp, z_cp); // translate so (r_j1, z_j1) is the origin of the coordinate system r_cp -= r_j1; z_cp -= z_j1; - // rtapi_print("inv: r_cp=%f, z_cp=%f (translated controlled point)\n", r_cp, z_cp); // // Now the origin (aka J1), J2, and CP define a triangle in the RZ plane. @@ -206,86 +198,23 @@ int kinematicsInverse( distance_to_cp = sqrt(pow(r_cp, 2) + pow(z_cp, 2)); distance_to_center = distance_to_cp / 2; - // rtapi_print("inv: distance to cp: %f\n", distance_to_cp); // find the angle of the vector from the origin to the CP angle_to_cp = TO_DEG * acos(r_cp / distance_to_cp); if (z_cp < 0) { angle_to_cp *= -1; } - // rtapi_print("inv: angle to cp: %f\n", angle_to_cp); // find the angle (Center, J1, J2) j1_angle = TO_DEG * acos(distance_to_center / L1_LENGTH); - // rtapi_print("inv: j1 angle: %f\n", j1_angle); joints[1] = angle_to_cp + j1_angle; - // rtapi_print("inv: j1: %f\n", joints[1]); // now we can compute the location of J2 z_j2 = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("inv: r_j2=%f, z_j2=%f (translated j2)\n", r_j2, z_j2); joints[2] = -1.0 * TO_DEG * asin((z_j2 - z_cp) / L2_LENGTH); - -#if 0 - // Distance between controlled point and the location of j1. These two - // points are separated by link 1, joint 1, and link 2. - distance_between_centers = sqrt(pow((r2 - r1), 2) + pow((z2 - z1), 2)); - - if (distance_between_centers > (L1_LENGTH + L2_LENGTH)) { - // trying to reach too far - return GO_RESULT_RANGE_ERROR; - } - - if (distance_between_centers < fabs(L1_LENGTH - L2_LENGTH)) { - // trying to reach too far into armpit - return GO_RESULT_RANGE_ERROR; - } - - delta = (1.0 / 4.0) * sqrt((distance_between_centers + L1_LENGTH + L2_LENGTH) * (distance_between_centers + L1_LENGTH - L2_LENGTH) * (distance_between_centers - L1_LENGTH + L2_LENGTH) * (L1_LENGTH + L2_LENGTH - distance_between_centers)); - - ir1 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - ir2 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - - iz1 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - iz2 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - - - // (ir1, iz1) is one intersection point, (ir2, iz2) is the other. - // These are the possible locations of the J2 joint. - // FIXME: For now we arbitrarily pick the one with the bigger Z. - - if (iz1 > iz2) { - j2_r = ir1; - j2_z = iz1; - } else { - j2_r = ir2; - j2_z = iz2; - } - // rtapi_print("inv: j2_r=%f, j2_z=%f (J2, intersection point)\n", j2_r, j2_z); - - // Make J1 point at J2 (j2_r, j2_z). - { - double l1_r = j2_r - r1; - joints[1] = TO_DEG * acos(l1_r / L1_LENGTH); - // rtapi_print("inv: l1_r=%f, j1=%f\n", l1_r, joints[1]); - } - - // Make J2 point at the controlled point. - { - double l2_r = r2 - j2_r; - double j2; - j2 = TO_DEG * acos(l2_r / L2_LENGTH); - if (j2_z > pose->tran.z) { - j2 *= -1; - } - joints[2] = j2; - // rtapi_print("inv: l2_r=%f, j2=%f\n", l2_r, joints[2]); - } -#endif - // A and B are wrist roll and pitch, handled in hal by external kinematics joints[3] = pose->a; joints[4] = pose->b; @@ -294,13 +223,14 @@ int kinematicsInverse( } -int kinematicsJacobian( +static int scorbot_jacobian( + const kins_params *p, const double *joints, const EmcPose *pose, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags ) { - // kinematicsInverse() above, differentiated step by step in the same + // scorbot_inverse() above, differentiated step by step in the same // order, each quantity carried as its gradient over (x, y, z) const double x = pose->tran.x, y = pose->tran.y; const double rho2 = x*x + y*y; @@ -310,6 +240,7 @@ int kinematicsJacobian( double q; int i, j, a; + (void)p; (void)joints; (void)iflags; if (rho2 <= 0) { return -1; } @@ -364,15 +295,26 @@ int kinematicsJacobian( return 0; } -KINEMATICS_TYPE kinematicsType(void) { - return KINEMATICS_BOTH; -} +static const kins_ops scorbot_ops = { + .forward = scorbot_forward, + .inverse = scorbot_inverse, + .jacobian = scorbot_jacobian, +}; + +// the arm's dimensions are the constants above; no geometry pins. The +// entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "scorbot-kins", + .halprefix = "scorbot-kins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZAB", + .max_joints = 5, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &scorbot_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -382,6 +324,10 @@ int rtapi_app_main(void) { if (comp_id < 0) { return comp_id; } + if (kinsSingleInit(comp_id, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; } @@ -389,4 +335,3 @@ int rtapi_app_main(void) { void rtapi_app_exit(void) { hal_exit(comp_id); } - diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index c58d726dd46..6f373cae2c8 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -4,10 +4,10 @@ * * Derived from a work by Fred Proctor * -* Author: +* Author: * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2004 All rights reserved. * * Last change: @@ -67,19 +67,15 @@ #include #include #include /* these decls */ +#include -/* ident tag */ -#ifndef __GNUC__ -#ifndef __attribute__ -#define __attribute__(x) -#endif -#endif - -static struct haldata { - hal_real_t bx; - hal_real_t cx; - hal_real_t cy; -} *haldata = NULL; +// the base geometry, one pin each, poked from HAL as before +static const kins_param_desc tripod_params[] = { + { "Bx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cy", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, +}; +enum { P_BX, P_CX, P_CY }; #define sq(x) ((x)*(x)) @@ -123,11 +119,13 @@ static struct haldata { solutions. Positive means the tripod is above the xy plane, negative means below. */ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int tripod_forward(const kins_params *p, kins_scratch *s_, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s_; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -137,9 +135,9 @@ int kinematicsForward(const double * joints, #define Dz (pos->tran.z) double P, Q, R; double s, t, u; - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; P = sq(AD); Q = sq(BD) - sq(Bx); @@ -183,11 +181,13 @@ int kinematicsForward(const double * joints, #undef Dz } -int kinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tripod_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -195,9 +195,9 @@ int kinematicsInverse(const EmcPose * pos, #define Dx (pos->tran.x) #define Dy (pos->tran.y) #define Dz (pos->tran.z) - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; AD = sqrt(sq(Dx) + sq(Dy) + sq(Dz)); BD = sqrt(sq(Dx - Bx) + sq(Dy) + sq(Dz)); @@ -218,14 +218,14 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tripod_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; /* the three strut base points, in the order of the joints */ const double base[3][2] = { {0, 0}, {Bx, 0}, {Cx, Cy} }; int i, j, a; @@ -249,170 +249,40 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -#ifdef MAIN - -#include -#include - -/* - Interactive testing of kins. - - Syntax: a.out -*/ -int main(int argc, char *argv[]) -{ -#ifndef BUFFERLEN -#define BUFFERLEN 256 -#endif - char buffer[BUFFERLEN]; - char cmd[BUFFERLEN]; - EmcPose pos, vel; - double joints[3]={0.0,0.0,0.0}, jointvels[3]={0.0,0.0,0.0}; - char inverse; - char flags; - KINEMATICS_FORWARD_FLAGS fflags; - - inverse = 0; /* forwards, by default */ - flags = 0; /* didn't provide flags */ - fflags = 0; /* above xy plane, by default */ - if (argc != 4 || - 1 != sscanf(argv[1], "%lf", &Bx) || - 1 != sscanf(argv[2], "%lf", &Cx) || - 1 != sscanf(argv[3], "%lf", &Cy)) { - fprintf(stderr, "syntax: %s Bx Cx Cy\n", argv[0]); - return 1; - } - - while (! feof(stdin)) { - if (inverse) { - printf("inv> "); - } - else { - printf("fwd> "); - } - fflush(stdout); - - if (NULL == fgets(buffer, BUFFERLEN, stdin)) { - break; - } - if (1 != sscanf(buffer, "%255s", cmd)) { - continue; - } - - if (! strcmp(cmd, "quit")) { - break; - } - if (! strcmp(cmd, "i")) { - inverse = 1; - continue; - } - if (! strcmp(cmd, "f")) { - inverse = 0; - continue; - } - if (! strcmp(cmd, "ff")) { - if (1 != sscanf(buffer, "%*s %lu", &fflags)) { - printf("need forward flag\n"); - } - continue; - } - - if (inverse) { /* inverse kins */ - if (3 != sscanf(buffer, "%lf %lf %lf", - &pos.tran.x, - &pos.tran.y, - &pos.tran.z)) { - printf("need X Y Z\n"); - continue; - } - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - } - } - } - else { /* forward kins */ - if (flags) { - if (4 != sscanf(buffer, "%lf %lf %lf %lu", - &joints[0], - &joints[1], - &joints[2], - &fflags)) { - printf("need 3 strut values and flag\n"); - continue; - } - } - else { - if (3 != sscanf(buffer, "%lf %lf %lf", - &joints[0], - &joints[1], - &joints[2])) { - printf("need 3 strut values\n"); - continue; - } - } - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - } - } - } - } /* end while (! feof(stdin)) */ - - return 0; -} - -#endif /* MAIN */ - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); +static const kins_ops tripod_ops = { + .forward = tripod_forward, + .inverse = tripod_inverse, + .jacobian = tripod_jacobian, +}; + +// three struts for three coordinates; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "tripodkins", + .halprefix = "tripodkins", + .params = tripod_params, + .nparams = sizeof(tripod_params)/sizeof(tripod_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &tripod_ops }, +}; MODULE_LICENSE("GPL"); - - static int comp_id; int rtapi_app_main(void) { - int res = 0; - comp_id = hal_init("tripodkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(struct haldata)); - if(!haldata) goto error; - - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->bx), 1.0, "tripodkins.Bx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cx), 1.0, "tripodkins.Cx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cy), 1.0, "tripodkins.Cy")) < 0) goto error; + if (kinsSingleInit(comp_id, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 8ad4ee1740e..865b70ece96 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -98,6 +98,7 @@ obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, % # -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 +matrixkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o millturn-extra-objs := $(SWITCHKINS_OBJS) xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) xyzacb_trsrn-extra-objs := $(SWITCHKINS_OBJS) diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index 8bf76899c8e..f3b1834bbb1 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -40,7 +40,7 @@ mechanical issues, including: 3. Parallelism between spindle rotational axis and Z movement. 4. Perpendicularity between spindle rotational axis and X/Y movement. -The matrix coefficients are set by parameters C_xx .. C_zz. +The matrix coefficients are set by the pins C_xx .. C_zz. For 3 axis machine, the equations become: .... @@ -152,7 +152,7 @@ Specify matrixkins in LinuxCNC INI file as: KINEMATICS=matrixkins ---- -In your HAL configuration file, set the parameters C_xx .. C_zz: +In your HAL configuration file, set the pins C_xx .. C_zz: [source,hal] ---- @@ -167,7 +167,7 @@ setp matrixkins.C_zy 0 # Skew Y axis towards Z axis setp matrixkins.C_zz 1 # Z axis scale ---- -The parameters can be modified during runtime using halcmd. +The pins can be modified during runtime using halcmd. To avoid sudden movements, it is better to turn off machine power before changes. If recalibration is performed with already existing calibration being in effect, @@ -180,68 +180,30 @@ option extra_setup; license "GPL"; ;; -static struct haldata { - hal_real_t C_xx; - hal_real_t C_xy; - hal_real_t C_xz; - hal_real_t C_yx; - hal_real_t C_yy; - hal_real_t C_yz; - hal_real_t C_zx; - hal_real_t C_zy; - hal_real_t C_zz; -} *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xx, 1.0, "matrixkins.C_xx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xy, 0.0, "matrixkins.C_xy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xz, 0.0, "matrixkins.C_xz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yx, 0.0, "matrixkins.C_yx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yy, 1.0, "matrixkins.C_yy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yz, 0.0, "matrixkins.C_yz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zx, 0.0, "matrixkins.C_zx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zy, 0.0, "matrixkins.C_zy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zz, 1.0, "matrixkins.C_zz"); - - if (res) goto error; - - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -} - #include -#include - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +#include + +// the calibration matrix, one pin each; the maths reads it from the block +static const kins_param_desc matrix_params[] = { + { "C_xx", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_xy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_xz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yy", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_yz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zz", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, +}; +enum { C_XX, C_XY, C_XZ, C_YX, C_YY, C_YZ, C_ZX, C_ZY, C_ZZ }; + +static int matrix_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // For forward kinematics (joint to axis position) we @@ -251,20 +213,20 @@ int kinematicsForward(const double *j, // https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/ // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); - - rtapi_real det = a * (e * i - f * h) - - b * (d * i - f * g) - + c * (d * h - e * g); - rtapi_real invdet = 1.0 / det; + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; + + const double det = a * (e * i - f * h) + - b * (d * i - f * g) + + c * (d * h - e * g); + const double invdet = 1.0 / det; // Apply inverse matrix transform to the 3 cartesian coordinates pos->tran.x = invdet * ( (e * i - f * h) * j[0] @@ -290,22 +252,24 @@ int kinematicsForward(const double *j, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int matrix_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; // Apply matrix transform to the 3 cartesian coordinates j[0] = pos->tran.x * a + pos->tran.y * b + pos->tran.z * c; @@ -323,10 +287,10 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int matrix_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int r, c; (void)j; @@ -337,15 +301,41 @@ int kinematicsJacobian(const double *j, } // the inverse is the calibration matrix itself, so its derivative is // that matrix, and the pass-through axes are ones - jac[0][0] = hal_get_real(haldata->C_xx); - jac[0][1] = hal_get_real(haldata->C_xy); - jac[0][2] = hal_get_real(haldata->C_xz); - jac[1][0] = hal_get_real(haldata->C_yx); - jac[1][1] = hal_get_real(haldata->C_yy); - jac[1][2] = hal_get_real(haldata->C_yz); - jac[2][0] = hal_get_real(haldata->C_zx); - jac[2][1] = hal_get_real(haldata->C_zy); - jac[2][2] = hal_get_real(haldata->C_zz); + jac[0][0] = p->geometry[C_XX]; + jac[0][1] = p->geometry[C_XY]; + jac[0][2] = p->geometry[C_XZ]; + jac[1][0] = p->geometry[C_YX]; + jac[1][1] = p->geometry[C_YY]; + jac[1][2] = p->geometry[C_YZ]; + jac[2][0] = p->geometry[C_ZX]; + jac[2][1] = p->geometry[C_ZY]; + jac[2][2] = p->geometry[C_ZZ]; for (r = 3; r < 9; r++) { jac[r][r] = 1; } return 0; } + +static const kins_ops matrix_ops = { + .forward = matrix_forward, + .inverse = matrix_inverse, + .jacobian = matrix_jacobian, +}; + +// the entry points come from kins_single.c, linked in +const kins_module_info kins_module = { + .name = "matrixkins", + .halprefix = "matrixkins", + .params = matrix_params, + .nparams = sizeof(matrix_params)/sizeof(matrix_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &matrix_ops }, +}; + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH); +} diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index ac0c003369d..f382b0f0544 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -16,9 +16,8 @@ where '2.8' is the branch name (use 'master' for the master branch). For a RIP (run-in-place) build, the file is located in the git tree as: `src/hal/components/userkins.comp`. -Edit the functions kinematicsForward() and kinematicsInverse() as required. - -If required, add HAL pins following examples in the template code. +Edit the functions userkins_forward() and userkins_inverse() as required, +and list the geometry the maths needs in the *userkins_params* table. Build and install the component using halcompile: @@ -50,16 +49,18 @@ change all instances of `userkins` to `mykins`. === NOTES +* The kinematics are written as functions of a parameter block, see + kinematics.h: the geometry is declared once in the *userkins_params* + table, one HAL pin is made per entry, and the maths reads + *p->geometry[]* where it would have read a pin. The classic entry + points (kinematicsForward() and the rest) are supplied by kins_single.c, + included below, so nothing here touches HAL and the same maths can be + evaluated outside realtime. * The *fpin* pin is included to satisfy the requirements of the halcompile utility but it is not accessible to kinematics functions. -* HAL pins and parameters needed in kinematics functions (kinematicsForward(), - kinematicsInverse()) must be setup in the *EXTRA_SETUP()* function, which - halcompile runs once when the module is loaded, before the component is - made ready. """; // The fpin pin is not accessible in kinematics functions. -// Use EXTRA_SETUP() 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; @@ -69,20 +70,22 @@ author "Dewey Garrett"; ;; #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; -} *haldata; -// hal pin/param types: -// hal_bool_t boolean bit -// hal_uint_t unsigned integer -// hal_sint_t signed integer -// hal_real_t floating point (double precision) +#include + +// the shared code for a module with one kinematics type, compiled in so +// that halcompile builds this file on its own +#include +#include + +// The geometry, one HAL pin per entry, named userkins.. An entry +// is an input (read into p->geometry[] before every call), an output +// (written from s->out[] after it), or an input that can be poked +// (KINS_IO). The example pair below echoes 'in' to 'out'. +static const kins_param_desc userkins_params[] = { + { "in", KINS_PARAM_U32, KINS_IN, 0, 0 }, + { "out", KINS_PARAM_U32, KINS_OUT, 0, 0 }, +}; +enum { P_IN, P_OUT }; FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -93,61 +96,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "userkins" - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(struct 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); - - if (res) goto error; - 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 -} - -KINS_NOT_SWITCHABLE -// see millturn.comp for example of switchable kinematics - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_IDENTITY; // 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 userkins_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; - static bool gave_msg; // [KINS]JOINTS=3 pos->tran.x = j[0]; // X coordinate pos->tran.y = j[1]; // Y coordinate @@ -160,23 +118,17 @@ 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() +} // userkins_forward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int userkins_inverse(const kins_params *p, kins_scratch *s, + 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 (3 required for this template). @@ -189,25 +141,26 @@ int kinematicsInverse(const EmcPose * pos, j[1] = pos->tran.y; // joint 1 j[2] = pos->tran.z; // joint 2 - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference - //read from param example: hal_set_ui32(haldata->out, hal_get_real(haldata->param_rw)); + // example output: echo the 'in' pin to the 'out' pin + s->out[P_OUT] = p->geometry[P_IN]; return 0; -} // kinematicsInverse() +} // userkins_inverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int userkins_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int r, c; + (void)p; (void)j; (void)pos; (void)iflags; // How each joint responds to each pose coordinate, the derivative of - // kinematicsInverse(): for this template joint 0 follows x, joint 1 + // userkins_inverse(): for this template joint 0 follows x, joint 1 // follows y and joint 2 follows z, each one for one. See kinematics.h. + // Leave .jacobian out of the ops below to have it differenced instead. for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } } @@ -215,4 +168,33 @@ int kinematicsJacobian(const double *j, jac[1][1] = 1; jac[2][2] = 1; return 0; -} // kinematicsJacobian() +} // userkins_jacobian() + +static const kins_ops userkins_ops = { + .forward = userkins_forward, + .inverse = userkins_inverse, + .jacobian = userkins_jacobian, + // .work, .tool and .native report the frames, see kinematics.h +}; + +const kins_module_info kins_module = { + .name = "userkins", + .halprefix = "userkins", + .params = userkins_params, + .nparams = sizeof(userkins_params)/sizeof(userkins_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &userkins_ops }, +}; + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects. KINEMATICS_IDENTITY is what +// kinematicsType() reports; use KINEMATICS_BOTH for a machine whose +// joints are not the axes, or to let a gui display joint values in the +// preview before homing. +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, "XYZ", KINEMATICS_IDENTITY); +} From 7e5fb3b5c24c71d41a6c2f20b736fe7a5338bec6 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:00:55 +1000 Subject: [PATCH 49/58] scarakins, pumakins, three21kins: move onto the parameter block Each arm declares its dimensions as a table, reads them from the block and registers one ops table for its own type, with the identity and userk types from the shared ops. pumakins keeps its flange frame and declares the half turn through the ops table's native rotation, where switchkinsRegisterFrames() carried it before. The setup functions and haldata go; pin names and defaults are unchanged. --- src/emc/kinematics/pumakins.c | 134 +++++++++++++---------------- src/emc/kinematics/scarakins.c | 142 ++++++++++++++----------------- src/emc/kinematics/three21kins.c | 117 ++++++++++++------------- 3 files changed, 177 insertions(+), 216 deletions(-) diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 5e381700a88..8958b919a5b 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -24,9 +24,15 @@ #include "pumakins.h" #include -struct haldata { - hal_real_t a2, a3, d3, d4, d6; -} *haldata = NULL; +// the five dimensions, one pin each; the maths reads them from the block +static const kins_param_desc puma_params[] = { + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A3 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D6 }, +}; +enum { P_A2, P_A3, P_D3, P_D4, P_D6 }; /* the difference of two angles, brought into (-pi, pi] so that a joint a whole turn from the formula still matches it */ @@ -107,11 +113,13 @@ static void pumaFlangeRotation(const double * joint, PmRotationMatrix * rot) *rot = hom.rot; } // pumaFlangeRotation() -static int pumaKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int puma_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double s1, s2, s3; double c1, c2, c3; @@ -135,10 +143,10 @@ static int pumaKinematicsForward(const double * joint, s23 = c2 * s3 + s2 * c3; c23 = c2 * c3 - s2 * s3; - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; /* Calculate term to be used in definition of... */ /* position vector. */ @@ -191,7 +199,7 @@ static int pumaKinematicsForward(const double * joint, *iflags |= PUMA_WRIST_FLIP; } } - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_D6 = p->geometry[P_D6]; /* add effect of d6 parameter */ hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6; hom.tran.y = hom.tran.y + hom.rot.z.y*PUMA_D6; @@ -210,32 +218,25 @@ static int pumaKinematicsForward(const double * joint, return 0; } -static int pumaKinematicsToolFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_tool_frame(const kins_params *p, const double * joint, + PmRotationMatrix * rot, + const KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)fflags; - // answers in the flange frame; switchkins applies the declared half turn + // answers in the flange frame; the declared half turn is applied by + // the shared code pumaFlangeRotation(joint, rot); return 0; -} // pumaKinematicsToolFrame() +} // puma_tool_frame() -static int pumaKinematicsWorkFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) -{ - (void)joint; - (void)fflags; - // the arm carries the tool and nothing carries the work - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // pumaKinematicsWorkFrame() - -static int pumaKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; @@ -271,11 +272,11 @@ static int pumaKinematicsInverse(const EmcPose * world, pmRpyQuatConvert(&rpy,&worldPose.rot); pmPoseHomConvert(&worldPose, &hom); - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; + const double PUMA_D6 = p->geometry[P_D6]; /* remove effect of d6 parameter */ px = hom.tran.x - PUMA_D6*hom.rot.z.x; @@ -388,29 +389,18 @@ static int pumaKinematicsInverse(const EmcPose * world, return 0; } -int pumaKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_PUMA560_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_PUMA560_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_PUMA560_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_PUMA560_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_PUMA560_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} // pumaKinematicsSetup() +// the arm carries the tool and nothing carries the work, so the work frame +// is the shared identity one. The maths is the ISO 9787 flange frame, so +// the tool axis it produces runs holder towards tip, the opposite of the +// convention; the declared half turn puts it right. No closed form +// Jacobian: the shared code differences the inverse. +static const kins_ops puma_ops = { + .forward = puma_forward, + .inverse = puma_inverse, + .work = kinsIdentityFrame, + .tool = puma_tool_frame, + .native = &TOOL_FRAME_FLANGE, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -418,29 +408,21 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "pumakins"; // !!! must agree with filename kp->halprefix = "pumakins"; // hal pin names kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = puma_params; + kp->nparams = sizeof(puma_params)/sizeof(puma_params[0]); rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = pumaKinematicsSetup; - *kfwd0 = pumaKinematicsForward; - *kinv0 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(0, &puma_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index ea86e3170ce..6e1b8beeb14 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -22,10 +22,6 @@ #include -static struct scara_data { - hal_real_t d1, d2, d3, d4, d5, d6; -} *haldata = NULL; - /* key dimensions joint[0] = Entire arm rotates around a vertical axis at its inner end @@ -62,13 +58,32 @@ static struct scara_data { on the value of joint[3]. */ +#define DEFAULT_D1 490 +#define DEFAULT_D2 340 +#define DEFAULT_D3 50 +#define DEFAULT_D4 250 +#define DEFAULT_D5 50 +#define DEFAULT_D6 50 + +// the six dimensions, one pin each; the maths reads them from the block +static const kins_param_desc scara_params[] = { + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D4 }, + { "D5", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D5 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D6 }, +}; +enum { P_D1, P_D2, P_D3, P_D4, P_D5, P_D6 }; + /* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */ -static -int scaraKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int scara_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double a0, a1, a3; double x, y, z, c; @@ -83,12 +98,12 @@ int scaraKinematicsForward(const double * joint, a1 = a1 + a0; a3 = a3 + a1; - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3); y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3); @@ -109,13 +124,15 @@ int scaraKinematicsForward(const double * joint, world->b = joint[5]; return (0); -} //scaraKinematicsForward() +} // scara_forward() -static int scaraKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int scara_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; double a3; double q0, q1; double xt, yt, rsq, cc; @@ -129,12 +146,12 @@ static int scaraKinematicsInverse(const EmcPose * world, /* convert degrees to radians */ a3 = c * ( PM_PI / 180 ); - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; /* center of end effector (correct for D6) */ xt = x - D6*cos(a3); @@ -176,17 +193,17 @@ static int scaraKinematicsInverse(const EmcPose * world, *fflags = 0; return (0); -} // scaraKinematicsInverse() +} // scara_inverse() -static int scaraKinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int scara_jacobian(const kins_params *p, const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)iflags; - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D2 = p->geometry[P_D2]; + const double D4 = p->geometry[P_D4]; + const double D6 = p->geometry[P_D6]; const double a3 = world->c * (PM_PI / 180); const double q1 = joint[1] * (PM_PI / 180); const double xt = world->tran.x - D6*cos(a3); @@ -228,38 +245,13 @@ static int scaraKinematicsJacobian(const double * joint, jac[4][3] = 1; jac[5][4] = 1; return 0; -} // scaraKinematicsJacobian() - -#define DEFAULT_D1 490 -#define DEFAULT_D2 340 -#define DEFAULT_D3 50 -#define DEFAULT_D4 250 -#define DEFAULT_D5 50 -#define DEFAULT_D6 50 - -static int scaraKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d5), DEFAULT_D5, "%s.D5", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; +} // scara_jacobian() -error: - return -1; -} // scaraKinematicsSetup() +static const kins_ops scara_ops = { + .forward = scara_forward, + .inverse = scara_inverse, + .jacobian = scara_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -267,25 +259,21 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "scarakins"; // !!! must agree with filename kp->halprefix = "scarakins"; // hal pin names kp->required_coordinates = "xyzabc"; // ab are scaragui table tilts kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = scara_params; + kp->nparams = sizeof(scara_params)/sizeof(scara_params[0]); rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = scaraKinematicsSetup; - *kfwd0 = scaraKinematicsForward; - *kinv0 = scaraKinematicsInverse; - switchkinsRegisterJacobian(0, scaraKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(0, &scara_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index b010eded5a7..e3e542f0dd7 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -27,9 +27,18 @@ /* flags for forward kinematics */ #define THREE21_REACH 0x01 -struct haldata { - hal_real_t a1, a2, a3, d1, d2, d3, d4, d6; -} *haldata = NULL; +// the eight dimensions, one pin each; the maths reads them from the block +static const kins_param_desc three21_params[] = { + { "A1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A1 }, + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A3 }, + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D6 }, +}; +enum { P_A1, P_A2, P_A3, P_D1, P_D2, P_D3, P_D4, P_D6 }; /* the difference of two angles, brought into (-pi, pi] so that a joint a whole turn from the formula still matches it */ @@ -41,20 +50,22 @@ static double angleDiff(double a, double b) return d; } -static int three21KinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int three21_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double s1, s2, s3, s4, s5, s6; double c1, c2, c3, c4, c5, c6; @@ -189,23 +200,25 @@ static int three21KinematicsForward(const double * joint, return 0; } -static int three21KinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int three21_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double t1, t2, t3; double k; @@ -348,31 +361,12 @@ static int three21KinematicsInverse(const EmcPose * world, return 0; } -int three21KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a1), DEFAULT_THREE21_A1, "%s.A1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_THREE21_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_THREE21_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_THREE21_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_THREE21_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_THREE21_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_THREE21_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_THREE21_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} +// no frames reported and no closed form Jacobian: the shared code +// differences the inverse +static const kins_ops three21_ops = { + .forward = three21_forward, + .inverse = three21_inverse, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -380,23 +374,20 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "three21kins"; kp->halprefix = "three21kins"; kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = three21_params; + kp->nparams = sizeof(three21_params)/sizeof(three21_params[0]); - *kset0 = three21KinematicsSetup; - *kfwd0 = three21KinematicsForward; - *kinv0 = three21KinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(0, &three21_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); return 0; } From 68e9cda2e74b2d89ba543acc88496b6e6f3ed072 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:06:35 +1000 Subject: [PATCH 50/58] millturn, xyzab_tdr_kins, xyzacb_trsrn, xyzbca_trsrn: move onto the parameter block Each component declares its geometry as a table, writes its types as ops over the block, and supplies switchkinsSetup() like the C modules do; EXTRA_SETUP() runs it through switchkinsRunSetup() and initialises, so the components link switchkins_setup.o too and export kinsDescribe() with the rest. The trsrn TCP type registers its frames and Jacobian in its ops table and the TOOL type the identity frames, as they were registered before. The inverses go on reading the rotary angles from their joint argument, as they always have. Pin names and defaults are unchanged. --- src/hal/components/Submakefile | 2 +- src/hal/components/millturn.comp | 101 +++--- src/hal/components/xyzab_tdr_kins.comp | 170 +++++---- src/hal/components/xyzacb_trsrn.comp | 467 +++++++++++-------------- src/hal/components/xyzbca_trsrn.comp | 433 ++++++++++------------- 5 files changed, 540 insertions(+), 633 deletions(-) diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 865b70ece96..d8975f3f8d1 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -97,7 +97,7 @@ obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, % # 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 +SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/switchkins_setup.o emc/kinematics/kins_util.o matrixkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o millturn-extra-objs := $(SWITCHKINS_OBJS) xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index ba13fa57886..b841ba3f0e0 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -26,7 +26,6 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// 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; @@ -49,22 +48,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -// the turn kinematics need no hal pins of their own -static int turnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; -} // turnKinematicsSetup() - -static int turnKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the turn kinematics: no geometry, written as pure functions of the +// parameter block (see kinematics.h) +static int turn_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; @@ -81,13 +74,16 @@ static int turnKinematicsForward(const double *j, pos->w = 0; return 0; -} // turnKinematicsForward() +} // turn_forward() -static int turnKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turn_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (void)iflags; (void)fflags; @@ -97,51 +93,62 @@ static int turnKinematicsInverse(const EmcPose * pos, j[3] = pos->a; return 0; -} // turnKinematicsInverse() +} // turn_inverse() -static int turnKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int turn_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int R, C; + (void)p; (void)j; (void)pos; (void)iflags; for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - // the derivative of turnKinematicsInverse(): which joint follows which - // pose coordinate, and in which sense + // the derivative of turn_inverse(): which joint follows which pose + // coordinate, and in which sense jac[2][0] = 1; jac[1][1] = -1; jac[0][2] = 1; jac[3][3] = 1; return 0; -} // turnKinematicsJacobian() +} // turn_jacobian() + +static const kins_ops turn_ops = { + .forward = turn_forward, + .inverse = turn_inverse, + .jacobian = turn_jacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "millturn"; + kp->halprefix = "millturn"; + kp->required_coordinates = "xyza"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &turn_ops); + return 0; +} // switchkinsSetup() // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + 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; } - if (switchkinsRegisterJacobian(1, turnKinematicsJacobian)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { 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 8b66b06af2d..ce21a7d8159 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -44,55 +44,33 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -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; - -static int tdrKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - 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) +// the geometry, one pin each; the maths reads it from the block (see +// kinematics.h), and the tool length from p->tool.tran.z +static const kins_param_desc tdr_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_XO, P_ZO, P_XR, P_YR, P_ZR }; + +static int tdr_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - 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 x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -125,22 +103,24 @@ static int tdrKinematicsForward(const double *j, pos->w = 0; return 0; -} // tdrKinematicsForward() +} // tdr_forward() -static int tdrKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdr_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - 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 x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - 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); + double dx = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -167,21 +147,21 @@ static int tdrKinematicsInverse(const EmcPose * pos, j[4] = pos->b; return 0; -} // tdrKinematicsInverse() +} // tdr_inverse() -static int tdrKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tdr_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - 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(tdrdata->x_offset); - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + double x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; + double dx = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; double sa = sin(pos->a*TO_RAD); double ca = cos(pos->a*TO_RAD); double sb = sin(pos->b*TO_RAD); @@ -195,9 +175,9 @@ static int tdrKinematicsJacobian(const double *j, for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - // tdrKinematicsInverse() differentiated: its coefficients of qx, qy - // and qz for the linear columns, and the same terms with a or b - // advanced a quarter turn for the rotary columns + // tdr_inverse() differentiated: its coefficients of qx, qy and qz for + // the linear columns, and the same terms with a or b advanced a + // quarter turn for the rotary columns jac[0][0] = cb; jac[0][1] = sa*sb; jac[0][2] = -ca*sb; @@ -217,30 +197,42 @@ static int tdrKinematicsJacobian(const double *j, jac[3][3] = 1; jac[4][4] = 1; return 0; -} // tdrKinematicsJacobian() +} // tdr_jacobian() + +static const kins_ops tdr_ops = { + .forward = tdr_forward, + .inverse = tdr_inverse, + .jacobian = tdr_jacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzab_tdr_kins"; + kp->halprefix = "xyzab_tdr_kins"; + kp->required_coordinates = "xyzab"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = tdr_params; + kp->nparams = sizeof(tdr_params)/sizeof(tdr_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tdr_ops); + return 0; +} // switchkinsSetup() // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + 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; } - if (switchkinsRegisterJacobian(1, tdrKinematicsJacobian)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { 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 2be983be778..8fb6d8dae39 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; 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 - // change them for demonstration purposes - hal_real_t y_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t y_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzacb_trsrn kinematics: - - // Declare hal pin pointers used for xyzacb_trsrn kinematics: - - hal_real_t tool_offset_z; -} *haldata; - -// 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) 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; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "y-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Ly = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Dray = (p)->geometry[P_ROT_AXIS] - (Dy + Ly); \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ // 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) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -130,8 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); @@ -144,32 +103,32 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dx - + (Cp*CvSs + Sp*r)*Ly - + Dy*Sp - + Dx + pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dx + + (Cp*CvSs + Sp*r)*Ly + + Dy*Sp + + Dx + Px; - pos->tran.y = - Cp*Cw*Dy - - Cw*Dx*Sp - - Cw*(Dray - Py) - - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly - + (Draz - Pz)*Sw - + Dray - + Dy + pos->tran.y = - Cp*Cw*Dy + - Cw*Dx*Sp + - Cw*(Dray - Py) + - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly + + (Draz - Pz)*Sw + + Dray + + Dy + Ly; - pos->tran.z = - Cp*Dy*Sw - - Dx*Sp*Sw - - Cw*(Draz - Pz) - - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) - + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly - - (Dray - Py)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = - Cp*Dy*Sw + - Dx*Sp*Sw + - Cw*(Draz - Pz) + - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) + + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly + - (Dray - Py)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; pos->b = j[4]; @@ -187,27 +146,27 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) - - (Cs*Ctc - CvSs*Stc)*Dx - + ((Ctc*CvSs + Stc*r)*Cp - + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) - - (Ctc*CvSs + Stc*r)*Dy - - (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) + - (Cs*Ctc - CvSs*Stc)*Dx + + ((Ctc*CvSs + Stc*r)*Cp + + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) + - (Ctc*CvSs + Stc*r)*Dy + - (Ctc*SvSs - Stc*t)*(Lz + Pz) - Ly*Stc; - pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) - + (Ctc*CvSs + Cs*Stc)*Dx - - ((CvSs*Stc - Ctc*r)*Cp - + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) - + (CvSs*Stc - Ctc*r)*Dy - - Ctc*Ly + pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) + + (Ctc*CvSs + Cs*Stc)*Dx + - ((CvSs*Stc - Ctc*r)*Cp + + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) + + (CvSs*Stc - Ctc*r)*Dy + - Ctc*Ly + (Stc*SvSs + Ctc*t)*(Lz + Pz); - pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) - + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) - - Dx*SvSs - + (Lz + Pz)*s - - Dy*t + pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) + + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) + - Dx*SvSs + + (Lz + Pz)*s + - Dy*t - Lz; pos->a = j[3]; @@ -222,47 +181,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, 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 - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); - - // substitutions as used in mathematical documentation - // including degree -> radians angle conversion + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -271,7 +218,7 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) double Cv = cos(nu*TO_RAD); double Stc = sin(tc); double Ctc = cos(tc); - + // in TCP we use the current positions of the spindle joints // in TOOL we will use the angle values calculated in remap.py double Ss = 0; @@ -286,10 +233,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // onLy used to be consistent with math in documentation double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // END of custom variable declaration for Forward kinematics + double Qy = pos->tran.y; + double Qz = pos->tran.z; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -303,25 +248,25 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - - Dy*Sp - - Dx + j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + - Dy*Sp + - Dx + Qx; - j[1] = Cp*Dy - + Dx*Sp - - Cw*(Dray + Dy + Ly - Qy) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Ly - - (Draz + Dt + Lz - Qz)*Sw + j[1] = Cp*Dy + + Dx*Sp + - Cw*(Dray + Dy + Ly - Qy) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Ly + - (Draz + Dt + Lz - Qz)*Sw + Dray; - j[2] = (Dt + Lz)*s - + Ly*t - - Cw*(Draz + Dt + Lz - Qz) - + (Dray + Dy + Ly - Qy)*Sw + j[2] = (Dt + Lz)*s + + Ly*t + - Cw*(Draz + Dt + Lz - Qz) + + (Dray + Dy + Ly - Qy)*Sw + Draz; j[3] = pos->a; @@ -329,80 +274,84 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) j[5] = pos->c; } 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); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - + (Cp*SvSs - Sp*t)*Lz - + ((Cp*Cs - CvSs*Sp)*Ctc - - (Cp*CvSs + Sp*r)*Stc)*Qx - - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy - + (Cp*SvSs - Sp*t)*Qz - - Dy*Sp - - Dx; - - j[1] = Cp*Dy - - (CvSs*Sp - Cp*r)*Ly - + (Sp*SvSs + Cp*t)*Lz - + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - + Dx*Sp - - Dy - - Ly; - - j[2] = - (Ctc*SvSs - Stc*t)*Qx - + (Stc*SvSs + Ctc*t)*Qy - + Lz*s - + Qz*s - + Ly*t - - Lz; - - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; + // in TOOL kinematics we use the articulated joint positions from the TWP + Ss = sin(theta_2*TO_RAD); + Cs = cos(theta_2*TO_RAD); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + j[0] = Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + + (Cp*SvSs - Sp*t)*Lz + + ((Cp*Cs - CvSs*Sp)*Ctc + - (Cp*CvSs + Sp*r)*Stc)*Qx + - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy + + (Cp*SvSs - Sp*t)*Qz + - Dy*Sp + - Dx; + + j[1] = Cp*Dy + - (CvSs*Sp - Cp*r)*Ly + + (Sp*SvSs + Cp*t)*Lz + + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + + Dx*Sp + - Dy + - Ly; + + j[2] = - (Ctc*SvSs - Stc*t)*Qx + + (Stc*SvSs + Ctc*t)*Qy + + Lz*s + + Qz*s + + Ly*t + - Lz; + + j[3] = pos->a; + j[4] = pos->b; + j[5] = pos->c; } return 0; } // trsrnInverse() -static int tcpKinematicsInverse(const EmcPose * pos, +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 1); + return trsrnInverse(p, pos, j, 1); } // toolKinematicsInverse() // The head answers in the convention already, so the native rotation -// registered with these frames is TOOL_FRAME_SPINDLE. -static int tcpKinematicsToolFrame(const double *j, +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees + double nu = p->geometry[P_NUT]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Ss = sin(j[4]*TO_RAD); @@ -437,10 +386,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { + (void)p; (void)fflags; double Sw = sin(j[3]*TO_RAD); double Cw = cos(j[3]*TO_RAD); @@ -456,23 +406,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +static int tcpKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - - // the same geometry as trsrnInverse(), read the same way - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -539,7 +481,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +static int toolKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) @@ -550,10 +492,10 @@ static int toolKinematicsJacobian(const double *j, // the head angles come from pins, so the inverse is linear in the pose // and the rows are its coefficients - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -592,40 +534,55 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, +}; + +// the tool kinematics report in tool axes, so the tool is square with the +// world by construction and nothing turns the work against it +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzacb_trsrn"; + kp->halprefix = "xyzacb_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() + // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + 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; } - if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, - tcpKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } - // the tool kinematics report in tool axes, so the tool is square with - // the world by construction and nothing turns the work against it - if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, - identityKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { 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 5b58e0afa7b..151fde7a352 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; 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 - // change them for demonstration purposes - hal_real_t x_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t x_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzbca_trsrn kinematics: - - // Declare hal pin pointers used for xyzbca_trsrn kinematics: - - hal_real_t tool_offset_z; -} *haldata; - -// 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) 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; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Lx = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Drax = (p)->geometry[P_ROT_AXIS] - Lx - Dx; \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ // 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) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx- Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -130,9 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); @@ -144,37 +102,33 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - // onLy used to be consistent with math in documentation - Px = j[0]; - Py = j[1]; - Pz = j[2]; - - pos->tran.x = - Cp*Cw*Dx - + Cw*Dy*Sp - - Cw*(Drax - Px) - - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx - - (Draz - Pz)*Sw - + Drax - + Dx + + pos->tran.x = - Cp*Cw*Dx + + Cw*Dy*Sp + - Cw*(Drax - Px) + - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx + - (Draz - Pz)*Sw + + Drax + + Dx + Lx; - pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dy - - (Cp*CvSs + Sp*r)*Lx - - Dx*Sp - + Dy + pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dy + - (Cp*CvSs + Sp*r)*Lx + - Dx*Sp + + Dy + Py; - pos->tran.z = Cp*Dx*Sw - - Dy*Sp*Sw - - Cw*(Draz - Pz) - + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) - - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx - + (Drax - Px)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = Cp*Dx*Sw + - Dy*Sp*Sw + - Cw*(Draz - Pz) + + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) + - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx + + (Drax - Px)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; pos->b = j[4]; @@ -192,27 +146,25 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) - + (CvSs*Stc - Ctc*r)*Dx - + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) - - (Ctc*CvSs + Cs*Stc)*Dy - - Ctc*Lx + pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) + + (CvSs*Stc - Ctc*r)*Dx + + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) + - (Ctc*CvSs + Cs*Stc)*Dy + - Ctc*Lx + (Stc*SvSs + Ctc*t)*(Lz + Pz); - - pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) - + (Ctc*CvSs + Stc*r)*Dx - + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) - - (Cs*Ctc - CvSs*Stc)*Dy - + (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) + + (Ctc*CvSs + Stc*r)*Dx + + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) + - (Cs*Ctc - CvSs*Stc)*Dy + + (Ctc*SvSs - Stc*t)*(Lz + Pz) + Lx*Stc; - - pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) - - (Cp*SvSs - Sp*t)*(Dy + Py) - + Dy*SvSs - + (Lz + Pz)*s - - Dx*t + pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) + - (Cp*SvSs - Sp*t)*(Dy + Py) + + Dy*SvSs + + (Lz + Pz)*s + - Dx*t - Lz; pos->a = j[3]; @@ -227,44 +179,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, 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 - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -288,11 +231,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // onLy used to be consistent with math in documentation double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // END of custom variable declaration for Forward kinematics - + double Qy = pos->tran.y; + double Qz = pos->tran.z; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -304,27 +244,27 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) SvSs = Sv*Ss; r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - Dy*Sp - - Cw*(Drax + Dx + Lx - Qx) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Lx - + (Draz + Dt + Lz - Qz)*Sw + t = Sv*Cv*(1-Cs); + + j[0] = Cp*Dx + - Dy*Sp + - Cw*(Drax + Dx + Lx - Qx) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Lx + + (Draz + Dt + Lz - Qz)*Sw + Drax; - j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - + Dx*Sp - - Dy + j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + + Dx*Sp + - Dy + Qy; - j[2] = (Dt + Lz)*s - + Lx*t - - Cw*(Draz + Dt + Lz - Qz) - - (Drax + Dx + Lx - Qx)*Sw + j[2] = (Dt + Lz)*s + + Lx*t + - Cw*(Draz + Dt + Lz - Qz) + - (Drax + Dx + Lx - Qx)*Sw + Draz; j[3] = pos->a; @@ -342,32 +282,31 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - (CvSs*Sp - Cp*r)*Lx - + (Sp*SvSs + Cp*t)*Lz - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx - - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - - Dy*Sp - - Dx + + j[0] = Cp*Dx + - (CvSs*Sp - Cp*r)*Lx + + (Sp*SvSs + Cp*t)*Lz + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx + - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + - Dy*Sp + - Dx - Lx; - j[1] = Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - - (Cp*SvSs - Sp*t)*Lz - + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx - + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy - - (Cp*SvSs - Sp*t)*Qz - + Dx*Sp + j[1] = Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + - (Cp*SvSs - Sp*t)*Lz + + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx + + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy + - (Cp*SvSs - Sp*t)*Qz + + Dx*Sp - Dy; - - j[2] = (Stc*SvSs + Ctc*t)*Qx - + (Ctc*SvSs - Stc*t)*Qy - + Lz*s - + Qz*s - + Lx*t + j[2] = (Stc*SvSs + Ctc*t)*Qx + + (Ctc*SvSs - Stc*t)*Qy + + Lz*s + + Qz*s + + Lx*t - Lz; j[3] = pos->a; @@ -378,34 +317,38 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) return 0; } // trsrnInverse() -static int tcpKinematicsInverse(const EmcPose * pos, +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 1); + return trsrnInverse(p, pos, j, 1); } // toolKinematicsInverse() // The head answers in the convention already, so the native rotation -// registered with these frames is TOOL_FRAME_SPINDLE. -static int tcpKinematicsToolFrame(const double *j, +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees + double nu = p->geometry[P_NUT]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Ss = sin(j[3]*TO_RAD); @@ -440,10 +383,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { + (void)p; (void)fflags; double Sw = sin(j[4]*TO_RAD); double Cw = cos(j[4]*TO_RAD); @@ -459,23 +403,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +static int tcpKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - - // the same geometry as trsrnInverse(), read the same way - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -542,7 +478,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +static int toolKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) @@ -553,10 +489,10 @@ static int toolKinematicsJacobian(const double *j, // the head angles come from pins, so the inverse is linear in the pose // and the rows are its coefficients - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -595,40 +531,55 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, +}; + +// the tool kinematics report in tool axes, so the tool is square with the +// world by construction and nothing turns the work against it +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzbca_trsrn"; + kp->halprefix = "xyzbca_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() + // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + 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; } - if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, - tcpKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } - // the tool kinematics report in tool axes, so the tool is square with - // the world by construction and nothing turns the work against it - if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, - identityKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() From 47dedfd9aa03fdebbeeb447809e041cf7b4dd412 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:13:52 +1000 Subject: [PATCH 51/58] genserkins, genhexkins, pentakins: move onto the parameter block The three that build a geometry from many pins now build it from the block on each call: genser its link description, genhex its base and platform points and joint axes, pentakins its base points and effector circles. What they reported through output pins, iteration counts, the strut corrections, the hexapod's fwd-kins-fail and the pose it found for a vismach gui, they report through the scratch, and the running maximum of iterations lives in the scratch too, so each caller keeps its own. genhex and pentakins declare their forward as iterating from the pose it is handed; the switchkins core seeds it with the last answer after a switch as before. genhexkins's six pose pins for the gui were created as inputs and written by the module; they are outputs now, which is the direction they were used in. pentakins declared its geometry as HAL parameters; the table makes them pins of the same names. genserfuncs.c loses the haldata, the global joint copy and the initialised flag, and the userspace test program ugenserkins builds a block itself and calls the ops. Pin names and defaults are otherwise unchanged. --- src/Makefile | 2 + src/emc/kinematics/Submakefile | 1 + src/emc/kinematics/genhexkins.c | 513 ++++++++++++++----------------- src/emc/kinematics/genserfuncs.c | 296 +++++++----------- src/emc/kinematics/genserkins.c | 23 +- src/emc/kinematics/genserkins.h | 36 +-- src/emc/kinematics/pentakins.c | 280 ++++++++--------- src/emc/kinematics/ugenserkins.c | 36 ++- 8 files changed, 510 insertions(+), 677 deletions(-) diff --git a/src/Makefile b/src/Makefile index ed4fd67bf88..2133f01e480 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1173,6 +1173,8 @@ lineardeltakins-objs += emc/kinematics/kins_single.o obj-m += pentakins.o pentakins-objs := emc/kinematics/pentakins.o +pentakins-objs += emc/kinematics/kins_util.o +pentakins-objs += emc/kinematics/kins_single.o pentakins-objs += libposemath/_posemath.o pentakins-objs += $(MATHSTUB) diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index c71c18696e2..dbbc783f21b 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -2,6 +2,7 @@ GENSERKINSSRCS := emc/kinematics/ugenserkins.c GENSERKINSSRCS += emc/kinematics/genserfuncs.c +GENSERKINSSRCS += emc/kinematics/kins_util.c USERSRCS += $(GENSERKINSSRCS) DELTAMODULESRCS := emc/kinematics/lineardeltakins.cc diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 848f88fde30..2adaeb08617 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -16,17 +16,17 @@ machines referred to as "Stewart Platforms". The functions are general enough to be configured for any platform - configuration. In the functions "genhexKinematicsForward" and - "genhexKinematicsInverse" are arrays "a[i]" and "b[i]". The values stored - in these arrays correspond to the positions of the ends of the i'th - strut. The value stored in a[i] is the position of the end of the i'th - strut attached to the platform, in platform coordinates. The value - stored in b[i] is the position of the end of the i'th strut attached - to the base, in base (world) coordinates. + configuration. In the functions "genhex_forward" and "genhex_inverse" + are arrays "a[i]" and "b[i]". The values stored in these arrays + correspond to the positions of the ends of the i'th strut. The value + stored in a[i] is the position of the end of the i'th strut attached + to the platform, in platform coordinates. The value stored in b[i] is + the position of the end of the i'th strut attached to the base, in + base (world) coordinates. The default values for base and platform joints positions are defined in the header file genhexkins.h. The actual values for a particular - machine can be adjusted by hal parameters: + machine can be adjusted by hal pins: genhexkins.base.N.x genhexkins.base.N.y @@ -67,18 +67,18 @@ genhexkins.correction.N - pins showing current values of strut length correction. - The genhexKinematicsInverse function solves the inverse kinematics using + The genhex_inverse function solves the inverse kinematics using a closed form algorithm. The inverse kinematics problem is given the pose of the platform and returns the strut lengths. For this problem there is only one solution that is always returned correctly. - The genhexKinematicsForward function solves the forward kinematics using + The genhex_forward function solves the forward kinematics using an iterative algorithm. Due to the iterative nature of this algorithm - the genhexKinematicsForward function requires an initial value to begin the + the genhex_forward function requires an initial value to begin the iterative routine and then converges to the "nearest" solution. The forward kinematics problem is given the strut lengths and returns the pose of the platform. For this problem there arein multiple - solutions. The genhexKinematicsForward function will return only one of + solutions. The genhex_forward function will return only one of these solutions which will be the solution nearest to the initial value given. It is possible that there are no solutions "near" the given initial value and the iteration will not converge and no @@ -103,6 +103,10 @@ genhexkins.max-iterations - maximum number of iterations spent for a converged solution during current session. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins above are the table below, read into the block + before every call and written from the scratch after it. + ----------------------------------------------------------------------------*/ #include @@ -114,49 +118,98 @@ #include "genhexkins.h" #include -static struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t platformx[NUM_STRUTS]; - hal_real_t platformy[NUM_STRUTS]; - hal_real_t platformz[NUM_STRUTS]; - hal_real_t basenx[NUM_STRUTS]; - hal_real_t baseny[NUM_STRUTS]; - hal_real_t basenz[NUM_STRUTS]; - hal_real_t platformnx[NUM_STRUTS]; - hal_real_t platformny[NUM_STRUTS]; - hal_real_t platformnz[NUM_STRUTS]; - hal_real_t correction[NUM_STRUTS]; - hal_real_t screw_lead; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; - hal_real_t spindle_offset; - hal_bool_t fwd_kins_fail; - - 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; - -} *haldata; - -static int genhex_gui_forward_kins(EmcPose *pos) -{ - hal_set_real(haldata->gui_x, pos->tran.x); - hal_set_real(haldata->gui_y, pos->tran.y); - hal_set_real(haldata->gui_z, pos->tran.z); - hal_set_real(haldata->gui_a, pos->a); - hal_set_real(haldata->gui_b, pos->b); - hal_set_real(haldata->gui_c, pos->c); - return 0; -} // genhex_gui_forward_kins +// the table: thirteen entries per strut, then the iteration controls, +// the offsets and the reports. The macros index it. +#define STRUT_ENTRIES 13 +#define P_BASE_X(i) (STRUT_ENTRIES*(i) + 0) +#define P_BASE_Y(i) (STRUT_ENTRIES*(i) + 1) +#define P_BASE_Z(i) (STRUT_ENTRIES*(i) + 2) +#define P_PLAT_X(i) (STRUT_ENTRIES*(i) + 3) +#define P_PLAT_Y(i) (STRUT_ENTRIES*(i) + 4) +#define P_PLAT_Z(i) (STRUT_ENTRIES*(i) + 5) +#define P_BASE_NX(i) (STRUT_ENTRIES*(i) + 6) +#define P_BASE_NY(i) (STRUT_ENTRIES*(i) + 7) +#define P_BASE_NZ(i) (STRUT_ENTRIES*(i) + 8) +#define P_PLAT_NX(i) (STRUT_ENTRIES*(i) + 9) +#define P_PLAT_NY(i) (STRUT_ENTRIES*(i) + 10) +#define P_PLAT_NZ(i) (STRUT_ENTRIES*(i) + 11) +#define P_CORR(i) (STRUT_ENTRIES*(i) + 12) +enum { + P_LAST_ITER = STRUT_ENTRIES*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_SPINDLE_OFFSET, + P_SCREW_LEAD, + P_GUI_X, P_GUI_Y, P_GUI_Z, P_GUI_A, P_GUI_B, P_GUI_C, + P_FWD_FAIL, + P_COUNT +}; + +#define STRUT_ROWS(i, bx, by, bz, px, py, pz, bnx, bny, bnz, pnx, pny, pnz) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "platform." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, px }, \ + { "platform." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, py }, \ + { "platform." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pz }, \ + { "base-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bnx }, \ + { "base-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, bny }, \ + { "base-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bnz }, \ + { "platform-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, pnx }, \ + { "platform-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, pny }, \ + { "platform-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pnz }, \ + { "correction." #i, KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 } + +static const kins_param_desc genhex_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, + DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_0_Z, + DEFAULT_BASE_0_NX, DEFAULT_BASE_0_NY, DEFAULT_BASE_0_NZ, + DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_0_NZ), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, + DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_1_Z, + DEFAULT_BASE_1_NX, DEFAULT_BASE_1_NY, DEFAULT_BASE_1_NZ, + DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_1_NZ), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, + DEFAULT_PLATFORM_2_X, DEFAULT_PLATFORM_2_Y, DEFAULT_PLATFORM_2_Z, + DEFAULT_BASE_2_NX, DEFAULT_BASE_2_NY, DEFAULT_BASE_2_NZ, + DEFAULT_PLATFORM_2_NX, DEFAULT_PLATFORM_2_NY, DEFAULT_PLATFORM_2_NZ), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, + DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_3_Z, + DEFAULT_BASE_3_NX, DEFAULT_BASE_3_NY, DEFAULT_BASE_3_NZ, + DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_3_NZ), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, + DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_4_Z, + DEFAULT_BASE_4_NX, DEFAULT_BASE_4_NY, DEFAULT_BASE_4_NZ, + DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_4_NZ), + STRUT_ROWS(5, DEFAULT_BASE_5_X, DEFAULT_BASE_5_Y, DEFAULT_BASE_5_Z, + DEFAULT_PLATFORM_5_X, DEFAULT_PLATFORM_5_Y, DEFAULT_PLATFORM_5_Z, + DEFAULT_BASE_5_NX, DEFAULT_BASE_5_NY, DEFAULT_BASE_5_NZ, + DEFAULT_PLATFORM_5_NX, DEFAULT_PLATFORM_5_NY, DEFAULT_PLATFORM_5_NZ), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IN, 0, 500.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IN, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IN, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + [P_SPINDLE_OFFSET] = { "spindle-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + [P_SCREW_LEAD] = { "screw-lead", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_SCREW_LEAD }, + // the pose the forward found, for a vismach gui; switchkins provides + // the skgui.* pins for the same purpose + [P_GUI_X] = { "x", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Y] = { "y", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Z] = { "z", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_A] = { "a", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_B] = { "b", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_C] = { "c", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_FWD_FAIL] = { "fwd-kins-fail", KINS_PARAM_BIT, KINS_OUT, 0, 0 }, +}; + +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert() ***************************/ @@ -259,45 +312,45 @@ static void MatMult(double J[][6], const double x[], double Ans[]) } } // MatMult() -/* declare arrays for base and platform coordinates */ -static PmCartesian b[NUM_STRUTS]; -static PmCartesian a[NUM_STRUTS]; - -/* declare base and platform joint axes vectors */ - -static PmCartesian nb1[NUM_STRUTS]; -static PmCartesian na0[NUM_STRUTS]; - -/************************genhex_read_hal_pins**************************/ - -static int genhex_read_hal_pins(void) { +/* the geometry of one call, taken from the block: base and platform + coordinates, the joint axes vectors and the screw lead */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + PmCartesian a[NUM_STRUTS]; + PmCartesian nb1[NUM_STRUTS]; + PmCartesian na0[NUM_STRUTS]; + double screw_lead; +} genhex_geometry; + +static void geometry_of(const kins_params *p, genhex_geometry *g) { int t; - /* set the base and platform coordinates from hal pin values */ - rtapi_real spindle_offset = hal_get_real(haldata->spindle_offset); - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + /* set the base and platform coordinates from the block */ + const double spindle_offset = p->geometry[P_SPINDLE_OFFSET]; + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + spindle_offset + tool_offset; - a[t].x = hal_get_real(haldata->platformx[t]); - a[t].y = hal_get_real(haldata->platformy[t]); - a[t].z = hal_get_real(haldata->platformz[t]) + spindle_offset + tool_offset; - - nb1[t].x = hal_get_real(haldata->basenx[t]); - nb1[t].y = hal_get_real(haldata->baseny[t]); - nb1[t].z = hal_get_real(haldata->basenz[t]); - na0[t].x = hal_get_real(haldata->platformnx[t]); - na0[t].y = hal_get_real(haldata->platformny[t]); - na0[t].z = hal_get_real(haldata->platformnz[t]); + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + spindle_offset + tool_offset; + g->a[t].x = p->geometry[P_PLAT_X(t)]; + g->a[t].y = p->geometry[P_PLAT_Y(t)]; + g->a[t].z = p->geometry[P_PLAT_Z(t)] + spindle_offset + tool_offset; + + g->nb1[t].x = p->geometry[P_BASE_NX(t)]; + g->nb1[t].y = p->geometry[P_BASE_NY(t)]; + g->nb1[t].z = p->geometry[P_BASE_NZ(t)]; + g->na0[t].x = p->geometry[P_PLAT_NX(t)]; + g->na0[t].y = p->geometry[P_PLAT_NY(t)]; + g->na0[t].z = p->geometry[P_PLAT_NZ(t)]; } - return 0; -} // genhex_read_hal_pins() + g->screw_lead = p->geometry[P_SCREW_LEAD]; +} // geometry_of() /***************************StrutLengthCorrection***************************/ -static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, +static int StrutLengthCorrection(const genhex_geometry *g, + const PmCartesian * StrutVectUnit, const PmRotationMatrix * RMatrix, const int strut_number, double * correction) @@ -306,32 +359,34 @@ static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, double dotprod; /* define base joints axis vectors */ - pmCartCartCross(&nb1[strut_number], StrutVectUnit, &nb2); + pmCartCartCross(&g->nb1[strut_number], StrutVectUnit, &nb2); pmCartCartCross(StrutVectUnit, &nb2, &nb3); pmCartUnitEq(&nb3); /* define platform joints axis vectors */ - pmMatCartMult(RMatrix, &na0[strut_number], &na1); + pmMatCartMult(RMatrix, &g->na0[strut_number], &na1); pmCartCartCross(&na1, StrutVectUnit, &na2); pmCartUnitEq(&na2); /* define dot product */ pmCartCartDot(&nb3, &na2, &dotprod); - *correction = hal_get_real(haldata->screw_lead) * asin(dotprod) / PM_2_PI; + *correction = g->screw_lead * asin(dotprod) / PM_2_PI; return 0; } // StrutLengthCorrection() -/**************** genhexKinematicsForward() *****************/ -static int genhexKinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +/**************** genhex_forward() *****************/ +static int genhex_forward(const kins_params *p, kins_scratch *s, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; + genhex_geometry g; PmCartesian aw; PmCartesian InvKinStrutVect,InvKinStrutVectUnit; PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; @@ -350,7 +405,7 @@ static int genhexKinematicsForward(const double * joints, int i; unsigned iteration = 0; - genhex_read_hal_pins(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ /* FIXME-- should check against triangle inequality, so that joints @@ -375,13 +430,16 @@ static int genhexKinematicsForward(const double * joints, q_trans.z = pos->tran.z; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +max_error) || (conv_err < -max_error)) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -2; }; @@ -389,9 +447,10 @@ static int genhexKinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -5; } @@ -402,18 +461,19 @@ static int genhexKinematicsForward(const double * joints, estimate to get joint estimate, subtract joints to get joint deltas, and compute inv J while we're at it */ for (i = 0; i < NUM_STRUTS; i++) { - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&q_trans, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -1; } pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.screw_lead != 0.0) { /* enable strut length correction */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); /* define corrected joint lengths */ InvKinStrutLength += corr; } @@ -454,7 +514,6 @@ static int genhexKinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -472,33 +531,42 @@ static int genhexKinematicsForward(const double * joints, pos->tran.y = q_trans.y; pos->tran.z = q_trans.z; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } - hal_set_bool(haldata->fwd_kins_fail, 0); + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); + s->out[P_FWD_FAIL] = 0; - genhex_gui_forward_kins(pos); + s->out[P_GUI_X] = pos->tran.x; + s->out[P_GUI_Y] = pos->tran.y; + s->out[P_GUI_Z] = pos->tran.z; + s->out[P_GUI_A] = pos->a; + s->out[P_GUI_B] = pos->b; + s->out[P_GUI_C] = pos->c; return 0; -} // genhexKinematicsForward() +} // genhex_forward() -/************************ genhexKinematicsInverse() ************************/ +/************************ genhex_inverse() ************************/ /* the inverse kinematics take world coordinates and determine joint values, given the inverse kinematics flags to resolve any ambiguities. The forward flags are set to indicate their value appropriate to the world coordinates passed in. */ -static int genhexKinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int genhex_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; + genhex_geometry g; PmCartesian aw, temp; PmCartesian InvKinStrutVect, InvKinStrutVectUnit; PmRotationMatrix RMatrix; @@ -506,7 +574,7 @@ static int genhexKinematicsInverse(const EmcPose * pos, int i; double InvKinStrutLength, corr; - genhex_read_hal_pins(); + geometry_of(p, &g); /* define Rotation Matrix */ rpy.r = pos->a * PM_PI / 180.0; @@ -518,22 +586,22 @@ static int genhexKinematicsInverse(const EmcPose * pos, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of platform strut end from platform to world coordinates */ - pmMatCartMult(&RMatrix, &a[i], &temp); + pmMatCartMult(&RMatrix, &g.a[i], &temp); pmCartCartAdd(&pos->tran, &temp, &aw); /* define strut lengths */ - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.screw_lead != 0.0) { /* enable strut length correction */ /* define unit strut vector */ if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { return -1; } /* define correction value and corrected joint lengths */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); - hal_set_real(haldata->correction[i], corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); + s->out[P_CORR(i)] = corr; InvKinStrutLength += corr; } @@ -541,9 +609,9 @@ static int genhexKinematicsInverse(const EmcPose * pos, } return 0; -} //genhexKinematicsInverse() +} //genhex_inverse() -/************************ genhexKinematicsJacobian() ***********************/ +/************************ genhex_jacobian() ***********************/ /* A strut length changes by the component of its platform end's motion along the strut. That end moves with the platform, dP + w x (R a), so the row for strut i is [u_i, (R a_i x u_i) . E] with u_i the unit strut @@ -551,11 +619,18 @@ static int genhexKinematicsInverse(const EmcPose * pos, words to the angular velocity w for R = Rz(c) Ry(b) Rx(a). The forward kinematics builds the same rows for its Newton step, in radians. */ -static int genhexKinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +// the inverse alone, for differencing where the closed form does not apply +static const kins_ops genhex_diff_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, +}; + +static int genhex_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + genhex_geometry g; PmCartesian aw, RMatrix_a, strut, u, moment; PmRotationMatrix RMatrix; PmRpy rpy; @@ -563,13 +638,14 @@ static int genhexKinematicsJacobian(const double * joints, double sb, cb, sc, cc; int i, j, col, m; - genhex_read_hal_pins(); + geometry_of(p, &g); /* the screw lead correction is a function of the pose too, and this does not differentiate it; difference the inverse instead */ - if (hal_get_real(haldata->screw_lead) != 0.0) { - return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS, - joints, pos, iflags, jac); + if (g.screw_lead != 0.0) { + kins_scratch scratch; + kinsScratchInit(&scratch); + return kinsOpsJacobian(&genhex_diff_ops, p, &scratch, joints, pos, jac, iflags); } for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { @@ -592,9 +668,9 @@ static int genhexKinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double len; - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &strut); + pmCartCartSub(&aw, &g.b[i], &strut); pmCartMag(&strut, &len); if (len <= 0) { return -1; } pmCartScalMult(&strut, 1.0/len, &u); @@ -610,145 +686,16 @@ static int genhexKinematicsJacobian(const double * joints, } } return 0; -} // genhexKinematicsJacobian() - -// HAL pin initializaion values. In small arrays so we can easily -// address them in the pin creation loop. -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, - DEFAULT_BASE_3_X, DEFAULT_BASE_4_X, DEFAULT_BASE_5_X, -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, - DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y, DEFAULT_BASE_5_Y, -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, - DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z, DEFAULT_BASE_5_Z, -}; -static const rtapi_real init_platformx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_2_X, - DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_5_X, -}; -static const rtapi_real init_platformy[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_2_Y, - DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_5_Y, +} // genhex_jacobian() + +// the forward iterates from the pose it is handed, so it is seeded with +// the last answer after a switch +static const kins_ops genhex_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, + .jacobian = genhex_jacobian, + .fwd_iterates = 1, }; -static const rtapi_real init_platformz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Z, DEFAULT_PLATFORM_1_Z, DEFAULT_PLATFORM_2_Z, - DEFAULT_PLATFORM_3_Z, DEFAULT_PLATFORM_4_Z, DEFAULT_PLATFORM_5_Z, -}; -static const rtapi_real init_basenx[NUM_STRUTS] = { - DEFAULT_BASE_0_NX, DEFAULT_BASE_1_NX, DEFAULT_BASE_2_NX, - DEFAULT_BASE_3_NX, DEFAULT_BASE_4_NX, DEFAULT_BASE_5_NX, -}; -static const rtapi_real init_baseny[NUM_STRUTS] = { - DEFAULT_BASE_0_NY, DEFAULT_BASE_1_NY, DEFAULT_BASE_2_NY, - DEFAULT_BASE_3_NY, DEFAULT_BASE_4_NY, DEFAULT_BASE_5_NY, -}; -static const rtapi_real init_basenz[NUM_STRUTS] = { - DEFAULT_BASE_0_NZ, DEFAULT_BASE_1_NZ, DEFAULT_BASE_2_NZ, - DEFAULT_BASE_3_NZ, DEFAULT_BASE_4_NZ, DEFAULT_BASE_5_NZ, -}; -static const rtapi_real init_platformnx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_2_NX, - DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_5_NX, -}; -static const rtapi_real init_platformny[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_2_NY, - DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_5_NY, -}; -static const rtapi_real init_platformnz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NZ, DEFAULT_PLATFORM_1_NZ, DEFAULT_PLATFORM_2_NZ, - DEFAULT_PLATFORM_3_NZ, DEFAULT_PLATFORM_4_NZ, DEFAULT_PLATFORM_5_NZ, -}; - -static -int genhexKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int i,res=0; - - if (kp->max_joints < 0 || kp->max_joints > NUM_STRUTS) { - rtapi_print_msg(RTAPI_MSG_ERR, "genhexKinematicsSetup: max_joints %d less than 0 or larger NUM_STRUTS %d\n", - kp->max_joints, NUM_STRUTS); - return -1; - } - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) { - rtapi_print_msg(RTAPI_MSG_ERR,"genhexKinematicsSetup: hal_malloc fail\n"); - return -1; - } - - for (i = 0; i < kp->max_joints; i++) { - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->basex[i]), - init_basex[i], "%s.base.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basey[i], - init_basey[i], "%s.base.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basez[i], - init_basez[i], "%s.base.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformx[i], - init_platformx[i], "%s.platform.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformy[i], - init_platformy[i], "%s.platform.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformz[i], - init_platformz[i], "%s.platform.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenx[i], - init_basenx[i], "%s.base-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->baseny[i], - init_baseny[i], "%s.base-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenz[i], - init_basenz[i], "%s.base-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnx[i], - init_platformnx[i], "%s.platform-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformny[i], - init_platformny[i], "%s.platform-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnz[i], - init_platformnz[i], "%s.platform-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_OUT, &haldata->correction[i], - 0.0, "%s.correction.%d", kp->halprefix, i); - if (res) {goto error;} - } - - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "genhexkins.last-iterations"); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "genhexkins.max-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->max_error, - 500.0, "genhexkins.max-error"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->conv_criterion, - 1e-9, "genhexkins.convergence-criterion"); - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->iter_limit, - 120, "genhexkins.limit-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "genhexkins.tool-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->spindle_offset, - 0.0, "genhexkins.spindle-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->screw_lead, - DEFAULT_SCREW_LEAD, "genhexkins.screw-lead"); - - if (res) {goto error;} - - //note: switchkins does not uses these as it provides gui.x, gui.y, etc. - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_x, 0.0, "genhexkins.x"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_y, 0.0, "genhexkins.y"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_z, 0.0, "genhexkins.z"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_a, 0.0, "genhexkins.a"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_b, 0.0, "genhexkins.b"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_c, 0.0, "genhexkins.c"); - - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->fwd_kins_fail, - 0, "genhexkins.fwd-kins-fail"); - - if (res) goto error; - return 0; - -error: - return res; -} // genhexKinematicsSetup() int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -756,6 +703,9 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "genhexkins"; // !!! must agree with filename kp->halprefix = "genhexkins"; // hal pin names kp->required_coordinates = "xyzabc"; @@ -763,21 +713,14 @@ int switchkinsSetup(kparms* kp, kp->allow_duplicates = 0; kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 kp->gui_kinstype = 0; //vismach gui for switchkins_type==0 + kp->params = genhex_params; + kp->nparams = P_COUNT; // switchkins_type==0 is startup default // kins with iterative forward algorithm should be switchkins_type==0 - *kset0 = genhexKinematicsSetup; - *kfwd0 = genhexKinematicsForward; - *kinv0 = genhexKinematicsInverse; - switchkinsRegisterJacobian(0, genhexKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(0, &genhex_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); return 0; } //switchkinsSetup() diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index d8432cdec3d..2f943d681aa 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -28,6 +28,11 @@ Currently the type of the joints is hardcoded to ANGULAR, although the kins support both ANGULAR and LINEAR axes. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins are the table below, read into the block + before every call, and the link description is built from the block + on each call. + TODO: * make number of joints a loadtime parameter * add HAL pins for all settable parameters, including joint type: ANGULAR / LINEAR @@ -48,44 +53,53 @@ #if __GNUC__ && !defined(__clang__) // The matrix and vector storage is just big. // genser_kin_jac_inv() is 2112 -// genserKinematicsInverse() is 2640 - #pragma GCC diagnostic warning "-Wframe-larger-than=2648" +// genser_inverse() is 2640 plus the link description it builds + #pragma GCC diagnostic warning "-Wframe-larger-than=3400" #endif -static struct haldata { - hal_uint_t max_iterations; - hal_uint_t last_iterations; - hal_real_t a[GENSER_MAX_JOINTS]; - hal_real_t alpha[GENSER_MAX_JOINTS]; - hal_real_t d[GENSER_MAX_JOINTS]; - hal_sint_t unrotate[GENSER_MAX_JOINTS]; - genser_struct *kins; - go_pose *pos; // used in various functions, we malloc it - // only once in genserKinematicsSetup() -} *haldata = NULL; - -static int total_joints; -double j[GENSER_MAX_JOINTS]; +// the table: four entries per joint, then the iteration count in and out +#define P_A(i) (4*(i) + 0) +#define P_ALPHA(i) (4*(i) + 1) +#define P_D(i) (4*(i) + 2) +#define P_UNROT(i) (4*(i) + 3) +enum { + P_LAST_ITER = 4*GENSER_MAX_JOINTS, + P_MAX_ITER, + P_COUNT +}; -#define KINS_PTR (haldata->kins) +#define JOINT_ROWS(i, a, alpha, d) \ + { "A-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, a }, \ + { "ALPHA-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, alpha }, \ + { "D-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, d }, \ + { "unrotate-" #i, KINS_PARAM_S32, KINS_IN, 0, 0 } + +const kins_param_desc GENSER_PARAMS[P_COUNT] = { + JOINT_ROWS(0, DEFAULT_A1, DEFAULT_ALPHA1, DEFAULT_D1), + JOINT_ROWS(1, DEFAULT_A2, DEFAULT_ALPHA2, DEFAULT_D2), + JOINT_ROWS(2, DEFAULT_A3, DEFAULT_ALPHA3, DEFAULT_D3), + JOINT_ROWS(3, DEFAULT_A4, DEFAULT_ALPHA4, DEFAULT_D4), + JOINT_ROWS(4, DEFAULT_A5, DEFAULT_ALPHA5, DEFAULT_D5), + JOINT_ROWS(5, DEFAULT_A6, DEFAULT_ALPHA6, DEFAULT_D6), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_IN, 0, GENSER_DEFAULT_MAX_ITERATIONS }, +}; +const int GENSER_NPARAMS = P_COUNT; #if GENSER_MAX_JOINTS < 6 #error GENSER_MAX_JOINTS must be at least 6; fix genserkins.h #endif -static int genser_hal_inited = 0; - -int genser_kin_init(void) { - genser_struct *genser = KINS_PTR; +void genser_links_of(const kins_params *p, genser_struct *genser) { int t; static volatile double tst=0;tst=sqrt(tst); // ensure -lm used /* init them all and make them revolute joints */ /* FIXME: should allow LINEAR joints based on HAL param too */ for (t = 0; t < GENSER_MAX_JOINTS; t++) { - genser->links[t].u.dh.a = hal_get_real(haldata->a[t]); - genser->links[t].u.dh.alpha = hal_get_real(haldata->alpha[t]); - genser->links[t].u.dh.d = hal_get_real(haldata->d[t]); + genser->links[t].u.dh.a = p->geometry[P_A(t)]; + genser->links[t].u.dh.alpha = p->geometry[P_ALPHA(t)]; + genser->links[t].u.dh.d = p->geometry[P_D(t)]; genser->links[t].u.dh.theta = 0; genser->links[t].type = GO_LINK_DH; genser->links[t].quantity = GO_QUANTITY_ANGLE; @@ -94,8 +108,13 @@ int genser_kin_init(void) { /* set a select few to make it PUMA-like */ // FIXME-AJ: make a hal pin, also set number of joints based on it genser->link_num = 6; + genser->iterations = 0; +} // genser_links_of() - return GO_RESULT_OK; +/* the unrotate coupling of one joint, from the block */ +static rtapi_s32 unrotate_of(const kins_params *p, int link) +{ + return (rtapi_s32)p->geometry[P_UNROT(link)]; } /* compute the forward jacobian function: @@ -314,7 +333,7 @@ int genser_kin_jac_fwd(void *kins, } /* The Jacobian in the terms of kinematics.h: joints in degrees per pose - word in EmcPose units, the derivative of genserKinematicsInverse(). + word in EmcPose units, the derivative of genser_inverse(). compute_jinv() gives the geometric inverse Jacobian, radians of joint per unit of base-frame twist. A pose word rate is not a twist: the roll, @@ -326,13 +345,14 @@ int genser_kin_jac_fwd(void *kins, with the unit conversions and the unrotate coupling applied in the order the inverse applies them. */ -int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int genser_jacobian(const kins_params *p, const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)iflags; - genser_struct *genser = KINS_PTR; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); go_pose T_L_0; @@ -342,14 +362,7 @@ int genserKinematicsJacobian(const double *joint, double sb, cb, sc, cc; int link, i, j, a, m, retval; -#ifndef ULAPI - genser_kin_init(); - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsJacobian: not initialized\n"); - return -1; - } -#endif + genser_links_of(p, genser); for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } @@ -358,7 +371,7 @@ int genserKinematicsJacobian(const double *joint, // the kinematic joint angles, in radians and with the unrotate // coupling removed, exactly as the forward prepares them for (link = 0; link < genser->link_num; link++) { - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); jest[link] = joint[link] * (PM_PI / 180); if (link && unrotate) jest[link] -= unrotate * jest[link-1]; @@ -404,7 +417,7 @@ int genserKinematicsJacobian(const double *joint, // the unrotate coupling, in link order as the inverse applies it for (link = 1; link < genser->link_num; link++) { - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); if (unrotate) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[link][a] += unrotate * jac[link-1][a]; @@ -413,86 +426,74 @@ int genserKinematicsJacobian(const double *joint, } // uvw pass through as joints 6, 7, 8 - if (total_joints > 6) jac[6][6] = 1; - if (total_joints > 7) jac[7][7] = 1; - if (total_joints > 8) jac[8][8] = 1; + if (p->max_joints > 6) jac[6][6] = 1; + if (p->max_joints > 7) jac[7][7] = 1; + if (p->max_joints > 8) jac[8][8] = 1; return 0; -} // genserKinematicsJacobian() +} // genser_jacobian() /* main function called by emc2 for forward Kins */ -int genserKinematicsForward(const double *joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) { +static int genser_forward(const kins_params *p, kins_scratch *s, + const double *joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - go_pose *pos; + genser_struct genser; + go_pose pos; go_rpy rpy; go_real jcopy[GENSER_MAX_JOINTS]; // will hold the radian conversion of joints int ret = 0; - int i, changed=0; - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsForward: not initialized\n"); - return -1; - } + int i; + + genser_links_of(p, &genser); for (i=0; i< 6; i++) { - // FIXME - debug hack - if (!GO_ROT_CLOSE(j[i],joint[i])) changed = 1; // convert to radians to pass to genser_kin_fwd jcopy[i] = joint[i] * PM_PI / 180; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[i]); + rtapi_s32 unrotate = unrotate_of(p, i); if ((i) && unrotate) jcopy[i] -= unrotate * jcopy[i-1]; } - if (changed) { - for (i=0; i< 6; i++) - j[i] = joint[i]; - // rtapi_print("genserKinematicsForward(joints: %f %f %f %f %f %f)\n", - //joint[0],joint[1],joint[2],joint[3],joint[4],joint[5]); - } // AJ: convert from emc2 coords (XYZABC - which are actually rpy euler // angles) // to go angles (quaternions) - pos = haldata->pos; rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &pos->rot); - pos->tran.x = world->tran.x; - pos->tran.y = world->tran.y; - pos->tran.z = world->tran.z; + go_rpy_quat_convert(&rpy, &pos.rot); + pos.tran.x = world->tran.x; + pos.tran.y = world->tran.y; + pos.tran.z = world->tran.z; //pass through unused 678 as uvw - if (total_joints > 6) world->u = joint[6]; - if (total_joints > 7) world->v = joint[7]; - if (total_joints > 8) world->w = joint[8]; + if (p->max_joints > 6) world->u = joint[6]; + if (p->max_joints > 7) world->v = joint[7]; + if (p->max_joints > 8) world->w = joint[8]; // pos will be the world location // jcopy: joitn position in radians - ret = genser_kin_fwd(KINS_PTR, jcopy, pos); + ret = genser_kin_fwd(&genser, jcopy, &pos); if (ret < 0) return ret; // AJ: convert back to emc2 coords - ret = go_quat_rpy_convert(&pos->rot, &rpy); + ret = go_quat_rpy_convert(&pos.rot, &rpy); if (ret < 0) return ret; - world->tran.x = pos->tran.x; - world->tran.y = pos->tran.y; - world->tran.z = pos->tran.z; + world->tran.x = pos.tran.x; + world->tran.y = pos.tran.y; + world->tran.z = pos.tran.z; world->a = rpy.r * 180 / PM_PI; world->b = rpy.p * 180 / PM_PI; world->c = rpy.y * 180 / PM_PI; - if (changed) { -// rtapi_print("genserKinematicsForward(world: %f %f %f %f %f %f)\n", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); - } return 0; } @@ -504,8 +505,6 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) int link; int retval; - genser_kin_init(); - for (link = 0; link < genser->link_num; link++) { retval = go_link_joint_set(&genser->links[link], joints[link], &linkout[link]); if (GO_RESULT_OK != retval) @@ -519,22 +518,25 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) return GO_RESULT_OK; } -int genserKinematicsInverse(const EmcPose * world, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int genser_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - genser_struct *genser = KINS_PTR; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); go_pose T_L_0; go_real dvw[6]; go_real jest[GENSER_MAX_JOINTS]; go_real dj[GENSER_MAX_JOINTS]; - go_pose pest, pestinv, Tdelta; // pos = converted pose from EmcPose + go_pose pos; // converted pose from EmcPose + go_pose pest, pestinv, Tdelta; go_rpy rpy; go_rvec rvec; go_cart cart; @@ -542,30 +544,19 @@ int genserKinematicsInverse(const EmcPose * world, int link; int smalls; int retval; + const unsigned max_iterations = (unsigned)p->geometry[P_MAX_ITER]; - // rtapi_print("kineInverse(joints: %f %f %f %f %f %f)\n", - // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); - // rtapi_print("kineInverse(world: %f %f %f %f %f %f)\n", - // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); - -#ifndef ULAPI - genser_kin_init(); - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsInverse: not initialized\n"); - return -1; - } -#endif + genser_links_of(p, genser); // FIXME-AJ: rpy or zyx ? rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &haldata->pos->rot); - haldata->pos->tran.x = world->tran.x; - haldata->pos->tran.y = world->tran.y; - haldata->pos->tran.z = world->tran.z; + go_rpy_quat_convert(&rpy, &pos.rot); + pos.tran.x = world->tran.x; + pos.tran.y = world->tran.y; + pos.tran.z = world->tran.z; go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); @@ -577,9 +568,10 @@ int genserKinematicsInverse(const EmcPose * world, } for (genser->iterations = 0; - genser->iterations < hal_get_ui32(haldata->max_iterations); + genser->iterations < max_iterations; genser->iterations++) { - hal_set_ui32(haldata->last_iterations, genser->iterations); + s->iterations = genser->iterations; + s->out[P_LAST_ITER] = genser->iterations; /* update the Jacobians */ for (link = 0; link < genser->link_num; link++) { go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); @@ -598,8 +590,7 @@ int genserKinematicsInverse(const EmcPose * world, } /* pest is the resulting pose estimate given joint estimate */ - genser_kin_fwd(KINS_PTR, jest, &pest); - //printf("jest: %f %f %f %f %f %f\n",jest[0],jest[1],jest[2],jest[3],jest[4],jest[5]); + genser_kin_fwd(genser, jest, &pest); /* pestinv is its inverse */ go_pose_inv(&pest, &pestinv); /* @@ -613,7 +604,7 @@ int genserKinematicsInverse(const EmcPose * world, .Tdelta = pestinv * pos L 0 L */ - go_pose_pose_mult(&pestinv, haldata->pos, &Tdelta); + go_pose_pose_mult(&pestinv, &pos, &Tdelta); /* We need Tdelta in 0 frame, not pest frame, so rotate it @@ -642,9 +633,9 @@ int genserKinematicsInverse(const EmcPose * world, go_matrix_vector_mult(&Jinv, dvw, dj); //pass through 678 as uvw - if (total_joints > 6) joints[6] = world->u; - if (total_joints > 7) joints[7] = world->v; - if (total_joints > 8) joints[8] = world->w; + if (p->max_joints > 6) joints[6] = world->u; + if (p->max_joints > 7) joints[7] = world->v; + if (p->max_joints > 8) joints[8] = world->w; /* check for small joint increments, if so we're done */ for (link = 0, smalls = 0; link < genser->link_num; link++) { @@ -661,14 +652,10 @@ int genserKinematicsInverse(const EmcPose * world, for (link = 0; link < genser->link_num; link++) { // convert from radians back to angles joints[link] = jest[link] * 180 / PM_PI; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); if ((link) && unrotate) joints[link] += unrotate * joints[link-1]; } - //rtapi_print("DONEkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n", - // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); - //rtapi_print("OKkineInverse: %.2f %.2f %.2f %.2f %.2f %.2f)\n", - // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); return GO_RESULT_OK; } /* else keep iterating */ @@ -682,6 +669,12 @@ int genserKinematicsInverse(const EmcPose * world, return GO_RESULT_ERROR; } +const kins_ops GENSER_OPS = { + .forward = genser_forward, + .inverse = genser_inverse, + .jacobian = genser_jacobian, +}; + /* Extras, not callable using go_kin_ wrapper but if you know you have linked in these kinematics, go ahead and call these for your ad hoc @@ -692,68 +685,3 @@ int genser_kin_inv_iterations(genser_struct * genser) { return genser->iterations; } - -int genser_kin_inv_set_max_iterations(int i) -{ - if (i <= 0) return GO_RESULT_ERROR; - hal_set_ui32(haldata->max_iterations, i); - return GO_RESULT_OK; -} - -int genser_kin_inv_get_max_iterations() -{ - return hal_get_ui32(haldata->max_iterations); -} - -static const rtapi_real init_a[GENSER_MAX_JOINTS] = { - DEFAULT_A1, DEFAULT_A2, DEFAULT_A3, DEFAULT_A4, DEFAULT_A5, DEFAULT_A6 -}; -static const rtapi_real init_alpha[GENSER_MAX_JOINTS] = { - DEFAULT_ALPHA1, DEFAULT_ALPHA2, DEFAULT_ALPHA3, DEFAULT_ALPHA4, DEFAULT_ALPHA5, DEFAULT_ALPHA6 -}; -static const rtapi_real init_d[GENSER_MAX_JOINTS] = { - DEFAULT_D1, DEFAULT_D2, DEFAULT_D3, DEFAULT_D4, DEFAULT_D5, DEFAULT_D6 -}; - - -int genserKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int i,res=0; - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) {goto error;} - - // allow for pass through joints 6,7,8 u,v,w - total_joints = kp->max_joints; - - // only the first 6 joints have A,ALPHA,D,unrotate pins - for (i = 0; i < GENSER_MAX_JOINTS; i++) { - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a[i]), - init_a[i], "%s.A-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->alpha[i]), - init_alpha[i], "%s.ALPHA-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d[i]), - init_d[i], "%s.D-%d", kp->halprefix, i); - res += hal_pin_new_si32(comp_id, HAL_IN, &(haldata->unrotate[i]), - 0, "%s.unrotate-%d", kp->halprefix, i); - } - res += hal_pin_new_ui32(comp_id, HAL_OUT, &(haldata->last_iterations), - 0, "%s.last-iterations",kp->halprefix); - - KINS_PTR = hal_malloc(sizeof(genser_struct)); - haldata->pos = (go_pose *) hal_malloc(sizeof(go_pose)); - if (KINS_PTR == NULL) {goto error;} - if (haldata->pos == NULL) {goto error;} - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->max_iterations, - GENSER_DEFAULT_MAX_ITERATIONS, "%s.max-iterations",kp->halprefix); - - if (res) {goto error;} - - genser_hal_inited = 1; - return 0; - -error: - return -1; -} // genserKinematicsSetup() diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index bdd37694030..bddbb492cba 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -4,7 +4,8 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions (setup,forward,inverse) +* 2) the maths and the geometry table are in genserfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ /******************************************************************** @@ -57,24 +58,20 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "genserkins"; // !!! must agree with filename kp->halprefix = "genserkins"; // hal pin names kp->required_coordinates = "xyzabcuvw"; // u,v,w are joints 6,7,8 kp->max_joints = strlen(kp->required_coordinates); kp->allow_duplicates = 0; + kp->params = GENSER_PARAMS; + kp->nparams = GENSER_NPARAMS; - *kset0 = genserKinematicsSetup; - *kfwd0 = genserKinematicsForward; - *kinv0 = genserKinematicsInverse; - switchkinsRegisterJacobian(0, genserKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(0, &GENSER_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index b74b826d2ec..c5a2d9526f8 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -81,8 +81,6 @@ typedef struct { extern int genser_kin_size(void); -extern int genser_kin_init(void); - extern const char * genser_kin_get_name(void); extern int genser_kin_num_joints(void * kins); @@ -125,15 +123,6 @@ extern int genser_kin_fwd_interations(genser_struct * genser); inverse kinematics functions */ extern int genser_kin_inv_iterations(genser_struct * genser); -/*! Sets the maximum number of iterations to use in future calls to - the inverse kinematics functions, after which an error will be - reported */ -extern int genser_kin_inv_set_max_iterations(int i); - -/*! Returns the maximum number of iterations that will be used to - compute inverse kinematics functions */ -extern int genser_kin_inv_get_max_iterations(void); - extern int compute_jfwd(go_link * link_params, int link_number, go_matrix * Jfwd, @@ -142,23 +131,14 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); -extern int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - -extern int genserKinematicsForward(const double *joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int genserKinematicsInverse(const EmcPose * world, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); +/* The kinematics as functions of the parameter block (see kinematics.h): + the DH parameters and the unrotate couplings are the table, the maths + is the ops. genser_links_of() fills a link description from a block, + for a caller that wants the go_ routines directly. */ +extern const kins_param_desc GENSER_PARAMS[]; +extern const int GENSER_NPARAMS; +extern const kins_ops GENSER_OPS; -extern int genserKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); +extern void genser_links_of(const kins_params *p, genser_struct *genser); #endif diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index f8415b4112c..42047ae8ac1 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -17,7 +17,7 @@ The default values for base and effector joints positions are defined in the header file pentakins.h. The actual values for a particular - machine can be adjusted by hal parameters: + machine can be adjusted by hal pins: pentakins.base.N.x pentakins.base.N.y @@ -45,6 +45,10 @@ pentakins.tool-offset - tool length from the origin along z axis, changes the effector pivot point. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins above are the table below, read into the block + before every call, and the entry points come from kins_single.c. + ----------------------------------------------------------------------------*/ #include @@ -52,23 +56,51 @@ #include #include #include /* these decls, KINEMATICS_FORWARD_FLAGS */ +#include #include "pentakins.h" -struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t effectorr[NUM_STRUTS]; - hal_real_t effectorz[NUM_STRUTS]; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; -} *haldata; +// the table: five struts' worth of geometry, then the iteration controls +// and reports. P_BASE_X(i) and the rest index it. +#define P_BASE_X(i) (5*(i) + 0) +#define P_BASE_Y(i) (5*(i) + 1) +#define P_BASE_Z(i) (5*(i) + 2) +#define P_EFF_R(i) (5*(i) + 3) +#define P_EFF_Z(i) (5*(i) + 4) +enum { + P_LAST_ITER = 5*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_COUNT +}; +#define STRUT_ROWS(i, bx, by, bz, er, ez) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "effector." #i ".r", KINS_PARAM_FLOAT, KINS_IN, 0, er }, \ + { "effector." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, ez } + +static const kins_param_desc penta_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_0_Z), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_1_Z), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_2_Z), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_3_Z), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, DEFAULT_EFFECTOR_4_R, DEFAULT_EFFECTOR_4_Z), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IO, 0, 100.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IO, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IO, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, +}; + +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert5() ***************************/ @@ -179,31 +211,29 @@ static double sqr(double x) return (x)*(x); } -/* declare arrays for base and effector coordinates */ -static PmCartesian b[NUM_STRUTS]; -static double za[NUM_STRUTS], ra[NUM_STRUTS]; - -/************************pentakins_read_hal_pins**************************/ +/* the base and effector geometry of one call, taken from the block */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + double za[NUM_STRUTS], ra[NUM_STRUTS]; +} penta_geometry; -int pentakins_read_hal_pins(void) { +static void geometry_of(const kins_params *p, penta_geometry *g) { int t; - - /* set the base and effector coordinates from hal pin values */ - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + tool_offset; - ra[t] = hal_get_real(haldata->effectorr[t]); - za[t] = hal_get_real(haldata->effectorz[t]) + tool_offset; + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + tool_offset; + g->ra[t] = p->geometry[P_EFF_R(t)]; + g->za[t] = p->geometry[P_EFF_Z(t)] + tool_offset; } - return 0; } /************************ InvKins() ********************************/ -int InvKins(const double * coord, - double * struts) +static int InvKins(const penta_geometry *g, + const double * coord, + double * struts) { PmCartesian xyz, pmcoord, temp; @@ -211,8 +241,6 @@ int InvKins(const double * coord, PmRpy rpy; int i; -// pentakins_read_hal_pins(); - /* define Rotation Matrix */ pmcoord.x = coord[0]; pmcoord.y = coord[1]; @@ -226,32 +254,30 @@ int InvKins(const double * coord, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of effector strut end from effector to world coordinates */ - pmCartCartSub(&b[i], &pmcoord, &temp); + pmCartCartSub(&g->b[i], &pmcoord, &temp); pmMatInv(&RMatrix, &InvRMatrix); pmMatCartMult(&InvRMatrix, &temp, &xyz); /* define strut lengths */ - struts[i] = sqrt( sqr(xyz.z - za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - ra[i]) ); + struts[i] = sqrt( sqr(xyz.z - g->za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - g->ra[i]) ); } return 0; } -/**************************** kinematicsForward() ***************************/ +/**************************** penta_forward() ***************************/ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int penta_forward(const kins_params *p, kins_scratch *s, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; -// PmCartesian aw; -// PmCartesian InvKinStrutVect,InvKinStrutVectUnit; -// PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; - + penta_geometry g; double Jacobian[NUM_STRUTS][NUM_STRUTS]; double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; double InvKinStrutLength[NUM_STRUTS], StrutLengthDiff[NUM_STRUTS]; @@ -260,14 +286,11 @@ int kinematicsForward(const double * joints, double coord[NUM_STRUTS]; double conv_err = 1.0; -// PmRotationMatrix RMatrix; -// PmRpy q_RPY; - int iterate = 1; int i, j; unsigned iteration = 0; - pentakins_read_hal_pins(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ if (joints[0] <= 0.0 || @@ -286,12 +309,15 @@ int kinematicsForward(const double * joints, coord[4] = pos->b * PM_PI / 180.0; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +(max_error)) || (conv_err < -(max_error))) { /* we can't converge */ + s->failed = 1; return -2; }; @@ -299,22 +325,23 @@ int kinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ + s->failed = 1; return -5; } /* compute StrutLengthDiff[] by running inverse kins on Cartesian estimate to get joint estimate, subtract joints to get joint deltas, and compute inv J while we're at it */ - InvKins(coord, InvKinStrutLength); + InvKins(&g, coord, InvKinStrutLength); for (i = 0; i < NUM_STRUTS; i++) { StrutLengthDiff[i] = InvKinStrutLength[i] - joints[i]; /* Build Inverse Jacobian Matrix */ coord[i] += 1e-4; - InvKins(coord, jointdelta); + InvKins(&g, coord, jointdelta); coord[i] -= 1e-4; for (j = 0; j < NUM_STRUTS; j++) { InverseJacobian[j][i] = (jointdelta[j] - InvKinStrutLength[j]) * 1e4; @@ -342,7 +369,6 @@ int kinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -357,34 +383,37 @@ int kinematicsForward(const double * joints, pos->a = coord[3] * 180.0 / PM_PI; pos->b = coord[4] * 180.0 / PM_PI; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); return 0; } -/************************ kinematicsInverse() ********************************/ +/************************ penta_inverse() ********************************/ /* the inverse kinematics take world coordinates and determine joint values, given the inverse kinematics flags to resolve any ambiguities. The forward flags are set to indicate their value appropriate to the world coordinates passed in. */ -/************************ kinematicsInverse() ********************************/ - -int kinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int penta_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; + penta_geometry g; double coord[NUM_STRUTS]; - pentakins_read_hal_pins(); + geometry_of(p, &g); coord[0] = pos->tran.x; coord[1] = pos->tran.y; @@ -392,18 +421,19 @@ int kinematicsInverse(const EmcPose * pos, coord[3] = pos->a * PM_PI / 180.0; coord[4] = pos->b * PM_PI / 180.0; - if (0 != InvKins(coord,joints)) { + if (0 != InvKins(&g, coord, joints)) { return -1; } return 0; } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int penta_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + penta_geometry g; PmRotationMatrix R; PmRpy rpy; PmCartesian P, d, xyz, wa, wb, dxyz[5]; @@ -411,7 +441,7 @@ int kinematicsJacobian(const double * joints, (void)joints; (void)iflags; - pentakins_read_hal_pins(); + geometry_of(p, &g); for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } } @@ -434,7 +464,7 @@ int kinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double rho, A, B, len; - pmCartCartSub(&b[i], &P, &d); + pmCartCartSub(&g.b[i], &P, &d); /* R^T d, written out since pmMatCartMult applies R */ xyz.x = R.x.x*d.x + R.x.y*d.y + R.x.z*d.z; xyz.y = R.y.x*d.x + R.y.y*d.y + R.y.z*d.z; @@ -461,8 +491,8 @@ int kinematicsJacobian(const double * joints, } rho = sqrt(sqr(xyz.x) + sqr(xyz.y)); - A = xyz.z - za[i]; - B = rho - ra[i]; + A = xyz.z - g.za[i]; + B = rho - g.ra[i]; len = sqrt(sqr(A) + sqr(B)); if (len <= 0 || rho <= 0) { return -1; } for (col = 0; col < 5; col++) { @@ -473,103 +503,43 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +// the forward iterates from the pose it is handed +static const kins_ops penta_ops = { + .forward = penta_forward, + .inverse = penta_inverse, + .jacobian = penta_jacobian, + .fwd_iterates = 1, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); +const kins_module_info kins_module = { + .name = "pentakins", + .halprefix = "pentakins", + .params = penta_params, + .nparams = P_COUNT, + .required_coordinates = "XYZAB", + .max_joints = NUM_STRUTS, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &penta_ops }, +}; MODULE_LICENSE("GPL"); int comp_id; -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, DEFAULT_BASE_3_X, DEFAULT_BASE_4_X -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z -}; -static const rtapi_real init_effectorr[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_4_R -}; -static const rtapi_real init_effectorz[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_Z, DEFAULT_EFFECTOR_1_Z, DEFAULT_EFFECTOR_2_Z, DEFAULT_EFFECTOR_3_Z, DEFAULT_EFFECTOR_4_Z -}; - int rtapi_app_main(void) { - int res = 0, i; - comp_id = hal_init("pentakins"); if (comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) - goto error; - - - for (i = 0; i < NUM_STRUTS; i++) { - - if ((res = hal_param_new_real(comp_id, HAL_RW, &(haldata->basex[i]), - init_basex[i], "pentakins.base.%d.x", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basey[i], - init_basey[i], "pentakins.base.%d.y", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basez[i], - init_basez[i], "pentakins.base.%d.z", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorr[i], - init_effectorr[i], "pentakins.effector.%d.r", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorz[i], - init_effectorz[i], "pentakins.effector.%d.z", i)) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "pentakins.last-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "pentakins.max-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->max_error, - 100.0, "pentakins.max-error")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->conv_criterion, - 1e-9, "pentakins.convergence-criterion")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_IO, &haldata->iter_limit, - 120, "pentakins.limit-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "pentakins.tool-offset")) < 0) - goto error; - hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } diff --git a/src/emc/kinematics/ugenserkins.c b/src/emc/kinematics/ugenserkins.c index 1d80e5ae70c..0d2c05b5a41 100644 --- a/src/emc/kinematics/ugenserkins.c +++ b/src/emc/kinematics/ugenserkins.c @@ -13,6 +13,7 @@ #include /* ulapi */ +#include #include /* struct timeval */ #include "genserkins.h" @@ -43,14 +44,25 @@ int main(int argc, char *argv[]) int retval = 0; double start, end; int comp_id; - kparms kp; - kp.max_joints = GENSER_MAX_JOINTS; - kp.allow_duplicates = 0; + kins_module_info info; + kins_params params; + kins_scratch scratch; - comp_id = hal_init("usergenserkins"); - if (genserKinematicsSetup(comp_id,"XYZABC",&kp)) printf("unexpected\n"); + /* the module described the way kinsDescribe() would, then a block at + the table defaults; setp has no say here */ + memset(&info, 0, sizeof(info)); + info.name = "genserkins"; + info.halprefix = "genserkins"; + info.params = GENSER_PARAMS; + info.nparams = GENSER_NPARAMS; + info.required_coordinates = "XYZABC"; + info.max_joints = GENSER_MAX_JOINTS; + info.ntypes = 1; + info.ops[0] = &GENSER_OPS; - genser_kin_init(); + comp_id = hal_init("usergenserkins"); + if (kinsParamsInit(¶ms, &info, "XYZABC")) printf("unexpected\n"); + kinsScratchInit(&scratch); /* syntax is a.out {i|f # # # # # #} */ if (argc == 8) { @@ -123,14 +135,14 @@ fprintf(stderr,"gki0:P %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", pos.tran.x,pos.tran.y,pos.tran.z,pos.a,pos.b,pos.c); fprintf(stderr,"gki1:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); - retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); + retval = GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); fprintf(stderr,"gki2:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); if (0 != retval) { printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); } } else { - retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + retval = GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); if (0 != retval) { printf("fwd kins error %d\n", retval); } @@ -220,14 +232,14 @@ joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); } else { fprintf(stderr,"gki1:\n"); retval = - genserKinematicsInverse(&pos, joints, &iflags, &fflags); + GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); printf("%f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); } else { retval = - genserKinematicsForward(joints, &pos, &fflags, &iflags); + GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); printf("%f %f %f %f %f %f\n", pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c); if (0 != retval) { @@ -271,13 +283,13 @@ fprintf(stderr,"gki1:\n"); &joints[0], &joints[1], &joints[2], &joints[3], &joints[4], &joints[5])) { printf("?\n"); } else { - retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + retval = GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); printf("xyzabc: %f %f %f %f %f %f\n", pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c); if (0 != retval) { printf("fwd kins error %d\n", retval); } else { - retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); + retval = GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); printf("j0--j5: %f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { From 4e2bfccb9276c1c794ba2caa4b92c0610aa8dc9e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:16:13 +1000 Subject: [PATCH 52/58] switchkins: seed the gui forward from the last answer for a pure type The gui forward for a parallel machine starts from the pose the main forward last found, which the older form saved in lastpose[]. The block path did not save it, so the gui forward of a type registered with switchkinsRegisterOps() started from zero and failed to converge, and genhexkins failed every forward. Save it on the block path too, and call the type's forward directly for the gui, without the seeding that belongs to the main call. --- src/emc/kinematics/switchkins.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 83e2445ec40..1c895bedb78 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -120,7 +120,8 @@ static void write_block(int ktype) kinsParamsPinsWrite(pins, kp.params, kp.nparams, &rt_scratch[ktype]); } -// the forward of one type, whichever way it was provided +// the forward of one type, whichever way it was provided, from the pose +// it is handed: no seeding, which is the caller's business static int call_forward(int ktype, const double *joint, EmcPose *pos, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags) @@ -128,8 +129,8 @@ static int call_forward(int ktype, const double *joint, EmcPose *pos, int r; if (kops[ktype]) { read_block(ktype); - r = kinsOpsForward(kops[ktype], &rt_params, &rt_scratch[ktype], - joint, pos, fflags, iflags); + r = kops[ktype]->forward(&rt_params, &rt_scratch[ktype], + joint, pos, fflags, iflags); write_block(ktype); return r; } @@ -217,7 +218,14 @@ int kinematicsForward(const double *joint, } if (kops[switchkins_type]) { - r = call_forward(switchkins_type, joint, pos, fflags, iflags); + read_block(switchkins_type); + r = kinsOpsForward(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, pos, fflags, iflags); + write_block(switchkins_type); + // the gui forward below starts from here, as it did for the + // older form + if (kops[switchkins_type]->fwd_iterates) {save_lastpose(switchkins_type,pos);} } else { if (fwd_iterates[switchkins_type] && use_lastpose[switchkins_type]) { // initialize iterative forward kins (ok for identity too) From 1cd891ce178bb7f5fd0a6194d61a8d179f085b15 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:57 +1000 Subject: [PATCH 53/58] kinematics_user: take the joints in as well as out, and bind modules lazily The loader zeroed the joint array before running a module's inverse and before an iterating forward. Motion hands a module the joints the machine is at, and some read them: a nutating head takes its rotary angles from there, the hexapod starts its forward from the pose it is handed. Zeros put the loader on a different branch from realtime for xyzacb_trsrn. The caller's joints are the seed now, the forward keeps the caller's pose as its seed when the module iterates, and the Jacobian runs its inverse from what the last inverse found. A halcompile component references hal_export_funct() and the rest of what its rtapi_app_main() needs, which only the realtime HAL library provides, so dlopen with RTLD_NOW refused every component. Nothing here calls that main; bind lazily and only what is called has to resolve. --- .../kinematics_userspace/kinematics_user.c | 24 +++++++++++++++---- .../kinematics_userspace/kinematics_user.h | 17 +++++++++---- 2 files changed, 33 insertions(+), 8 deletions(-) diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index b1ccec60eca..637bc2de12f 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -57,6 +57,7 @@ struct KinematicsUserContext { int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ int tool_param; /* the table's tool entry, -1 if none */ int warned_tool; + double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ }; /* ======================================================================== @@ -290,7 +291,11 @@ static int load_module(KinematicsUserContext *ctx, snprintf(module_path, sizeof(module_path), "%s/rtlib/%s.so", EMC2_HOME, module_name); - handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + /* lazily: a halcompile component references hal_export_funct() and + the rest of what its rtapi_app_main() needs, which only the realtime + HAL library provides, and nothing here calls that main. What is + called, kinsDescribe() and the ops, resolves when it is called. */ + handle = dlopen(module_path, RTLD_LAZY | RTLD_LOCAL); if (!handle) { fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", module_path, dlerror()); @@ -431,12 +436,18 @@ int kinematicsUserInverse(KinematicsUserContext* ctx, if (ctx->rt_only) return -1; refresh(ctx); - for (i = 0; i < EMCMOT_MAX_JOINTS; i++) j[i] = 0.0; + /* the joints go in as well as out: motion hands a module where the + machine is, and some read that (a nutating head takes its rotary + angles from it), so the caller's array is the seed */ + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; + } if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, world, j, &iflags, &fflags) != 0) { return -1; } for (i = 0; i < ctx->num_joints; i++) joints[i] = j[i]; + memcpy(ctx->last_joints, j, sizeof(ctx->last_joints)); return 0; } @@ -456,7 +467,11 @@ int kinematicsUserForward(KinematicsUserContext* ctx, for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; } - memset(world, 0, sizeof(*world)); + /* a forward that iterates starts from the pose it is handed, so the + caller's world is the seed; any other gets a clean one */ + if (!ctx->info.ops[ctx->ktype]->fwd_iterates) { + memset(world, 0, sizeof(*world)); + } return kinsOpsForward(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, world, &fflags, &iflags); } @@ -475,7 +490,8 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, if (ctx->rt_only) return -1; refresh(ctx); - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) j[r] = 0.0; + /* the joints at this pose, on the branch the last inverse was on */ + memcpy(j, ctx->last_joints, sizeof(j)); if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, world, j, &iflags, &fflags) != 0) { return -1; diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 0a7187537f9..3d1e8c2bf8f 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -90,9 +90,14 @@ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); /** * Perform inverse kinematics (world coords -> joint positions) * + * The joint array goes in as well as out: motion hands a module the + * joints the machine is at, and a module may read them (a nutating + * head takes its rotary angles from there, an iterating inverse starts + * there), so pass the current joints, not zeros. + * * @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] + * @param joints Joint positions in and out [KINEMATICS_USER_MAX_JOINTS] * @return 0 on success, -1 on failure */ int kinematicsUserInverse(KinematicsUserContext* ctx, @@ -102,9 +107,12 @@ int kinematicsUserInverse(KinematicsUserContext* ctx, /** * Perform forward kinematics (joint positions -> world coords) * + * A module whose forward iterates (the hexapod, the pentapod) starts + * from the pose in *world, so hand it one near the answer. + * * @param ctx Kinematics context from kinematicsUserInit * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] - * @param world Output world coordinates + * @param world Output world coordinates, and the seed on input * @return 0 on success, -1 on failure */ int kinematicsUserForward(KinematicsUserContext* ctx, @@ -114,8 +122,9 @@ int kinematicsUserForward(KinematicsUserContext* ctx, /** * The Jacobian at a pose, J[joint][axis] = d joint / d axis, from the * module's closed form where it has one and by differencing its inverse - * where it does not. The inverse is run at the pose first, so the - * derivative is taken on the solution branch the module picks there. + * where it does not. The inverse is run at the pose first, seeded with + * what the last kinematicsUserInverse() found, so the derivative is + * taken on the solution branch the caller is on. * * @return 0 on success, -1 on failure */ From eadbac5c472517c04f42b7d1e3e95563de758ef1 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:57 +1000 Subject: [PATCH 54/58] switchkinscomp: write the template on the parameter block The out-of-tree template declares its geometry as a table, writes its example type as ops over the block and supplies switchkinsSetup() like the in-tree modules, with EXTRA_SETUP() running it through switchkinsRunSetup(). It includes switchkins_setup.c alongside the other two sources, so that file joins those installed in share/linuxcnc. The kparms it built was never zeroed, which the grown struct would have turned into a crash. --- .gitignore | 1 + debian/linuxcnc-uspace-dev.install | 1 + src/Makefile | 2 +- src/emc/kinematics/Submakefile | 1 + src/hal/components/switchkinscomp.comp | 134 +++++++++++++++---------- 5 files changed, 84 insertions(+), 55 deletions(-) diff --git a/.gitignore b/.gitignore index 19647ebbccd..e3f96f3a77a 100644 --- a/.gitignore +++ b/.gitignore @@ -22,6 +22,7 @@ share/desktop-directories/linuxcnc-ref.directory share/desktop-directories/linuxcnc-doc.directory share/linuxcnc/mesa_modbus.c.tmpl share/linuxcnc/switchkins.c +share/linuxcnc/switchkins_setup.c share/linuxcnc/kins_util.c share/linuxcnc/kins_single.c src/modules.order diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index 251c401a9e9..e13585d482d 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -6,5 +6,6 @@ usr/lib/*.so usr/share/linuxcnc/Makefile.modinc usr/share/linuxcnc/mesa_modbus.c.tmpl usr/share/linuxcnc/switchkins.c +usr/share/linuxcnc/switchkins_setup.c usr/share/linuxcnc/kins_util.c usr/share/linuxcnc/kins_single.c diff --git a/src/Makefile b/src/Makefile index 2133f01e480..7d7474fcf61 100644 --- a/src/Makefile +++ b/src/Makefile @@ -777,7 +777,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 ../src/emc/kinematics/kins_single.c $(DESTDIR)$(prefix)/share/linuxcnc/ + $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/switchkins_setup.c ../src/emc/kinematics/kins_util.c ../src/emc/kinematics/kins_single.c $(DESTDIR)$(prefix)/share/linuxcnc/ install-kernel-indep: install-python install-python: install-dirs diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index dbbc783f21b..89c6173d5be 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -40,6 +40,7 @@ PYTARGETS += $(RDELTAMODULE) # in-tree ones link it. EMCKINEMATICSSRCS = \ ../share/linuxcnc/switchkins.c \ + ../share/linuxcnc/switchkins_setup.c \ ../share/linuxcnc/kins_util.c \ ../share/linuxcnc/kins_single.c diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index 7e90edc380f..e5ca4034b9c 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -17,6 +17,12 @@ 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. +The kinematics are written as functions of a parameter block, see +kinematics.h and the Kinematics Conventions chapter: the geometry is +declared once in a table, one HAL pin is made per entry, and the maths +reads the block where it would have read a pin. The same maths can +then be evaluated outside realtime. + 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*). @@ -53,11 +59,15 @@ option extra_setup; // 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. Both are installed with the -// headers, so halcompile finds them with no path of your own. +// switchkins_setup.c runs the switchkinsSetup() below and provides +// kinsDescribe() for a copy of the module loaded outside realtime. +// kins_util.c provides the identity kinematics, the parameter block +// helpers and the coordinates letters-to-joints mapping. All are +// installed with the headers, so halcompile finds them with no path of +// your own. #include +#include #include //===================================================================== @@ -66,36 +76,31 @@ 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; +// The geometry: one HAL pin per entry, named ., read +// into the block before every call. Add whatever the real kinematics +// need; an entry flagged as the tool arrives in p->tool.tran.z as well. - return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, - "%s.x-offset", kp->halprefix); -} // myKinematicsSetup() +static const kins_param_desc my_params[] = { + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_X_OFFSET }; -static int myKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +//--------------------------------------------------------------------- +// Example switchkins-type: a forward and an inverse over the block. +// Replace the arithmetic with the real kinematics. The frames and the +// Jacobian are optional, see kinematics.h. + +static int myForward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - pos->tran.x = j[0] + hal_get_real(mydata->x_offset); + pos->tran.x = j[0] + p->geometry[P_X_OFFSET]; pos->tran.y = j[1]; pos->tran.z = j[2]; @@ -104,50 +109,71 @@ static int myKinematicsForward(const double *j, pos->u = pos->v = pos->w = 0; return 0; -} // myKinematicsForward() +} // myForward() -static int myKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int myInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - j[0] = pos->tran.x - hal_get_real(mydata->x_offset); + j[0] = pos->tran.x - p->geometry[P_X_OFFSET]; j[1] = pos->tran.y; j[2] = pos->tran.z; return 0; -} // myKinematicsInverse() +} // myInverse() + +static const kins_ops my_ops = { + .forward = myForward, + .inverse = myInverse, +}; + +//--------------------------------------------------------------------- +// The module's configuration and its switchkins-types. Type 0 is the +// startup default. Types run from 0 to SWITCHKINS_MAX_TYPES-1 with no +// gaps. + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + // the pointer arguments are the older way of providing types 0 to 2 + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + + 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->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); + + if (switchkinsRegisterOps(0, &KINS_IDENTITY_OPS)) { return -1; } + if (switchkinsRegisterOps(1, &my_ops)) { return -1; } + return 0; +} // switchkinsSetup() //--------------------------------------------------------------------- // 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. +// switchkinsInit() expects, so setup is run 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; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() From c539195855ee1685027332991f188024bfaedad7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:58 +1000 Subject: [PATCH 55/58] docs: describe the parameter block form of a kinematics module The conventions chapter gains a section on the two blocks, the table, the ops table and what the shared code does with them in and outside realtime, and the Writing a Module list gains "no state". The frames and Jacobian sections point at the ops table where they pointed at the register calls. The switchkins chapter's Code Notes describe switchkinsRegisterOps(), the table in kparms, switchkinsRunSetup() and kinsDescribe(), keep the older registration as the older form, and the outline is a module written the new way. --- docs/src/motion/kinematics-conventions.adoc | 95 ++++++++++++- docs/src/motion/switchkins.adoc | 139 ++++++++++++-------- 2 files changed, 176 insertions(+), 58 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 84ea1b09a5d..72858070537 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -178,7 +178,7 @@ half turn about one of the two transverse axes, and which one is chosen decides where tool X lands. Because it is a rotation in its own right, a module declares it rather than -applying it by hand, as the last argument of `switchkinsRegisterFrames()`. +applying it by hand, in the `native` field of its ops table. Shared code applies it and checks once, at load, that it is orthonormal with determinant +1. `TOOL_FRAME_SPINDLE` is the identity, for a module whose maths is already in the convention; `TOOL_FRAME_FLANGE` is the half turn a @@ -289,6 +289,8 @@ All of these are functions of the joint values and the module's own geometry. None needs state carried between calls, and none needs the module to be running in a realtime thread to be useful: the interesting callers, a limit check before a move and a preview before a program runs, are not in the servo loop. +<> is how a module is written so that they +can call it. [[sec:orientation-inverse]] == The Orientation Inverse @@ -446,9 +448,9 @@ flags select. That costs a few microseconds on a closed form inverse and milliseconds on one that iterates, and it answers to the inverse's own precision, which for an iterating inverse is its convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type -that registers nothing; an identity type answers exactly. +whose ops table has no Jacobian; an identity type answers exactly. -A module with a closed form registers it with `switchkinsRegisterJacobian()`. +A module with a closed form puts it in the `jacobian` field of its ops table. It is exact, it costs what the inverse costs, and it knows its own singular poses rather than discovering them as an inverse that fails a step away from the pose. Every module in the tree whose inverse is written out supplies one. @@ -460,6 +462,86 @@ rather than from the pose, which the nutating heads do, has an inverse whose derivative about the pose is not the coupling the machine has. Such a module supplies the closed form, taken against the pose. +[[sec:parameters]] +== The Parameter Block + +Everything above is a function of the joint values, the tool and the machine's +geometry. A module written the old way reads its geometry from HAL pins it +created, keeps its kinematics type and its iteration scratch in statics, and so +can only answer for the machine as it is now, from inside the realtime thread. +Anything else that needs the same maths, a planner evaluating poses the machine +has not reached, task checking a program at load, a tool asking what if, had to +carry a second copy of it, and the two copies drift. + +A module is written instead as functions of two blocks the caller supplies. +`kins_params` describes the machine: the kinematics type, the joint map from +`coordinates=`, the tool offset, and the geometry as an array of doubles. One +copy may be shared by any number of callers, since nothing writes it during a +call. `kins_scratch` is what one caller carries between its own calls: the pose +an iterating forward last found, which seeds the next, and what the module +reports about the call it just made. It is never shared between callers, so +motion and a planner evaluating the same module cannot disturb each other. + +=== The table + +A module declares its geometry as a table of named entries, one per value it +reads. The name is the pin name the config already uses, less the module +prefix, so nothing in a config changes. + +[source,c] +---- +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, +}; +enum { P_PIVOT_LENGTH }; +---- + +An entry is an input, an output, or an input that can be poked (`KINS_IO`, a +`HAL_IO` pin). The maths reads `p->geometry[P_PIVOT_LENGTH]` where it read a +pin, and writes an output into `s->out[]` at the same index. An entry flagged +as the tool is the tool length along the tool axis; the shared code puts its +value in `p->tool.tran.z` as well, which is what the maths reads, so that a +caller outside realtime can supply the tool from the tool table without there +being a pin. + +=== The ops table + +The maths of one kinematics type is a `kins_ops` table: the forward and inverse, +the optional work and tool frames with the native rotation that relates the +tool frame to the convention, and the optional Jacobian. A type whose forward +iterates from the pose it is handed says so, and the shared code seeds it with +the last answer after a switch. A module with several types has one geometry +table and one ops table per type, registered with `switchkinsRegisterOps()`; a +module with one type describes itself in a `kins_module` and links +`kins_single.c`. + +=== What the shared code does + +In realtime it makes one HAL pin per table entry, copies the pins into the +block before every call and the outputs back after it, and supplies the classic +entry points, `kinematicsForward()` and the rest, so that motion sees no +difference. Outside realtime a module exports `kinsDescribe()`, which hands a +caller its table and the ops of each type; the caller fills a block from +wherever it likes and asks the same functions through `kinsOpsForward()`, +`kinsOpsInverse()`, `kinsOpsJacobian()` and the frame calls, with the same +defaults applied, so both sides get the same answers. The non-realtime loader +in `kinematics_userspace/` binds the pins of the running module by the table's +names and takes the tool from motion's own offset pins, and says once when the +module's tool pin disagrees with them, which is a config that lost the tool on +the way. `kinslimits` is built on it. + +A module that does not provide the form keeps working as it did. It just cannot +be evaluated outside realtime, which the loader reports. + +=== What stays outside the block + +The kinematics type is in the block, so a caller evaluating a program that +switches type puts the type each block will run under in its own block, and +nothing is switched globally. The tool is in the block, from motion. The joint +map is in the block, from `coordinates=`. Nothing else the maths needs exists, +and a module that finds it needs something else has found a parameter it should +declare. + [[sec:writing-a-module]] == Writing a Module @@ -500,6 +582,13 @@ Geometry stays in the module:: the module. A consumer that restates it has taken a copy that nothing keeps in step, which is the situation this chapter exists to end. +No state:: + Write the maths as functions of the parameter block and the scratch, as + <> describes: geometry in the table, + the kinematics type and the tool from the block, and anything carried + between calls in the scratch. A static in a module is a second machine + that only the realtime thread can see. + Mount orientation is not this:: A tool or holder mount orientation is a different quantity: a right-angle head, a tool held at an angle, an end effector clocked on its flange. Those diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index a81b0dd01d4..989645ba58b 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -416,7 +416,8 @@ 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 directory and edited to supply custom kinematics with -kinstype==2. +kinstype==2: the in-tree modules register its USERK_OPS as that +kinstype, so the forward and inverse in the copy are what runs. The user custom kinematics file can be compiled from out-of-tree source locations for rt-preempt implementations or by replacing @@ -445,18 +446,20 @@ is included: [source,c] ---- #include +#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 +as source: switchkins.c, switchkins_setup.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 -calls. +The module supplies switchkinsSetup() and calls switchkinsRunSetup() +and switchkinsInit() from EXTRA_SETUP(), which halcompile runs after +hal_init() and before hal_ready(). See <> for the calls. ---- $ halcompile --install user_switchkins.comp @@ -508,17 +511,28 @@ 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: +A kinstype is supplied by calling switchkinsRegisterOps(), once per +kinstype, with the maths of that type written as functions of the +parameter block (see the Kinematics Conventions chapter): + +---- +int switchkinsRegisterOps(int ktype, const kins_ops *ops); +---- + +The geometry of the whole module is one table, named in the kparms +fields 'params' and 'nparams'; every kinstype reads it from the block. +The older form, switchkinsRegister() with a setup, forward and inverse +routine per kinstype that read pins of their own, is still accepted: ---- int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- 'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in -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. +switchkins.h). Registering a kinstype twice, by either route, 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. @@ -529,15 +543,15 @@ 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. +which checks the supplied parameters, creates the HAL pins, the +table's among them, selects kinstype 0, and then invokes the setup +routine of each kinstype registered the older way. 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. +A module built this way also exports kinsDescribe(), through which a +copy of it loaded outside realtime learns its table and the maths of +each kinstype; the non-realtime loader and kinslimits use it. === Module main program @@ -554,31 +568,59 @@ int switchkinsSetup(kparms* kp, 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. +which sets the configuration settings, names the geometry table and +registers the kinstypes with switchkinsRegisterOps(). The pointer +arguments are the older route for kinstypes 0,1,2; a module using +them leaves the rest alone. switchkinsRunSetup() in +switchkins_setup.c is what runs switchkinsSetup() and registers what +it returned, for the 'main' program and for kinsDescribe() alike. 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: +from halcompile instead. It supplies the same switchkinsSetup(), and +from its EXTRA_SETUP() routine, which halcompile runs after +hal_init() and before hal_ready(), calls switchkinsRunSetup() and +then switchkinsInit(). The component names the objects it needs in +hal/components/Submakefile: ---- -millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/switchkins_setup.o emc/kinematics/kins_util.o ---- === Outline -The two routes in one switchkinsSetup(), with the kinematics itself -left out. Types 0 to 2 are filled in through the pointer arguments as -they always were, and a fourth is registered: +A switchkinsSetup() with the kinematics itself left out: the table, +the ops table of the machine's own kinstype, and the shared identity +and userk ops for the other two: [source,c] ---- +static const kins_param_desc my_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, 100.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, // the tool length +}; +enum { P_PIVOT_LENGTH, P_TOOL_OFFSET }; + +static int my_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double pivot = p->geometry[P_PIVOT_LENGTH]; // where a pin was read + double tool = p->tool.tran.z; // the tool, from wherever the caller has it + // ... +} + +static int my_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +static const kins_ops my_ops = { + .forward = my_forward, + .inverse = my_inverse, + // .work, .tool, .native and .jacobian are optional, see kinematics.h +}; + int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, @@ -589,37 +631,24 @@ int switchkinsSetup(kparms* kp, kp->halprefix = "mykins"; // hal pin names kp->required_coordinates = "xyzab"; kp->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); // remaining kparms fields - *kset0 = identityKinematicsSetup; // kinstype 0 is the startup default - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = myKinematicsSetup; - *kfwd1 = myKinematicsForward; - *kinv1 = myKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; - - // any further kinstype comes from switchkinsRegister(), and the - // numbering carries on from the three above with no gaps - if (switchkinsRegister(3, myOtherKinematicsSetup, - myOtherKinematicsForward, - myOtherKinematicsInverse)) { return -1; } + switchkinsRegisterOps(0, &my_ops); // kinstype 0 is the startup default + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); + // any further kinstype is registered the same way, and the + // numbering carries on with no gaps return 0; } // switchkinsSetup() ---- -A module wanting fewer than three kinstypes leaves the unused pointer -arguments alone and starts registering at the first free number. - For the surrounding shape, the in-tree switchkinsSetup() routines are in src/emc/kinematics: 5axiskins.c, xyzac-trt-kins.c, genserkins.c, scarakins.c and the others listed at the top of this document. None of -them registers a fourth kinstype yet, so the call above has no in-tree +them registers a fourth kinstype yet, so a call for one has no in-tree example to copy. // vim: set syntax=asciidoc: From 491a7082cbebae07bc01112d9a5d3236697c5a78 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:58 +1000 Subject: [PATCH 56/58] tests: check that a module answers the same outside realtime tests/kins-params loads each module, evaluates it once in realtime through the classic entry points (paritycheck.c publishes the forward, the inverse and the Jacobian on pins) and once through the non-realtime loader from python (check.py: kinsDescribe(), the block filled from the module's pins, the same ops), and requires the same success and the same numbers to rounding. 34 runs over the 24 modules, every switchable type that has geometry of its own, the parallel machines from a pose their forward can be seeded with. Checked by mutation: the loader not refreshing the geometry fails 10 comparisons in the first module with a table; the loader seeding the inverse with zeros instead of the caller's joints fails the three translation joints of xyzacb_trsrn, which reads its rotary angles from that array. --- tests/kins-params/check.py | 136 +++++++++++++++++++++++++++ tests/kins-params/checkresult | 4 + tests/kins-params/paritycheck.c | 148 +++++++++++++++++++++++++++++ tests/kins-params/skip | 4 + tests/kins-params/test.sh | 161 ++++++++++++++++++++++++++++++++ 5 files changed, 453 insertions(+) create mode 100755 tests/kins-params/check.py create mode 100755 tests/kins-params/checkresult create mode 100644 tests/kins-params/paritycheck.c create mode 100755 tests/kins-params/skip create mode 100755 tests/kins-params/test.sh diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py new file mode 100755 index 00000000000..b6f7758f892 --- /dev/null +++ b/tests/kins-params/check.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python3 +# The non-realtime half of the parameter block parity test. +# +# Evaluates the module paritycheck was loaded after through the +# non-realtime loader (libkinslimits, kinematicsUserInit and friends), +# which dlopens the module, asks it to describe itself through +# kinsDescribe(), fills a parameter block from the module's own pins and +# calls the same ops the realtime wrapper calls. The answers have to +# match what paritycheck published, to rounding, or the module is not +# the pure function of its parameters it claims to be. +# +# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM +# POSE and JNT are comma separated numbers, as given to paritycheck; +# COORDS and SPARM are a dash when the module was loaded without them. + +import ctypes +import os +import sys + +import hal + +EMC2_HOME = os.environ.get("EMC2_HOME") +# global, so the module the loader dlopens resolves its HAL and RTAPI +# symbols against the same library +def lib(name): + if EMC2_HOME: + return ctypes.CDLL(os.path.join(EMC2_HOME, "lib", name), mode=ctypes.RTLD_GLOBAL) + return ctypes.CDLL(name, mode=ctypes.RTLD_GLOBAL) + +class EmcPose(ctypes.Structure): + _fields_ = [(n, ctypes.c_double) for n in "xyzabcuvw"] + +MAX_JOINTS = 9 +AXES = 9 +Joints = ctypes.c_double * MAX_JOINTS +Jac = (ctypes.c_double * AXES) * MAX_JOINTS + +module, joints, coords, ktype, frompose = sys.argv[1], int(sys.argv[2]), sys.argv[3], int(sys.argv[4]), int(sys.argv[5]) +pose_in = [float(v) for v in sys.argv[6].split(",")] +jnt_in = [float(v) for v in sys.argv[7].split(",")] +# a dash stands for an absent value, since halcmd hands quotes through +if coords == "-": + coords = "" +sparm = sys.argv[8].encode() if len(sys.argv) > 8 and sys.argv[8] not in ("", "-") else None +pose_in += [0.0] * (AXES - len(pose_in)) +jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) + +halc = lib("liblinuxcnchal.so.0") +kins = lib("libkinslimits.so.0") + +kins.kinematicsUserInitSparm.restype = ctypes.c_void_p +kins.kinematicsUserInitSparm.argtypes = [ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p, + ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p] +for fn in ("kinematicsUserIsRtOnly", "kinematicsUserGetNumTypes"): + getattr(kins, fn).argtypes = [ctypes.c_void_p] +kins.kinematicsUserSetType.argtypes = [ctypes.c_void_p, ctypes.c_int] +kins.kinematicsUserInverse.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Joints] +kins.kinematicsUserForward.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(EmcPose)] +kins.kinematicsUserJacobian.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Jac] +kins.kinematicsUserFree.argtypes = [ctypes.c_void_p] + +comp_id = halc.hal_init(b"kpcheck") +if comp_id < 0: + print("kins-params: FAIL hal_init") + sys.exit(1) +ctx = kins.kinematicsUserInitSparm(module.encode(), joints, coords.encode() if coords else None, sparm, + comp_id, b"kpcheck") +halc.hal_ready(comp_id) +failures = 0 + +def fail(what): + global failures + failures += 1 + print("kins-params: FAIL %s" % what) + +if not ctx or kins.kinematicsUserIsRtOnly(ctx): + fail("%s cannot be evaluated outside realtime" % module) + sys.exit(1) +if ktype and kins.kinematicsUserSetType(ctx, ktype): + fail("%s has no type %d in the block form" % (module, ktype)) + sys.exit(1) + +def pose_of(values): + p = EmcPose() + for n, v in zip("xyzabcuvw", values): + setattr(p, n, v) + return p + +def close(a, b): + return abs(a - b) <= 1e-9 * max(1.0, abs(a), abs(b)) + +def compare(what, ours, theirs): + if not close(ours, theirs): + fail("%s: loader %.12g, realtime %.12g" % (what, ours, theirs)) + +rc_fwd = hal.get_value("paritycheck.rc-fwd") +rc_inv = hal.get_value("paritycheck.rc-inv") +rc_jac = hal.get_value("paritycheck.rc-jac") + +q = Joints(*jnt_in) +qi = Joints(*jnt_in) +J = Jac() +if frompose: + P = pose_of(pose_in) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(P), qi) + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, qi, ctypes.byref(F)) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(P), J) +else: + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, q, ctypes.byref(F)) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(F), qi) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(F), J) + +# the same success or failure on both sides, then the same numbers +for what, ours, theirs in (("forward", r_fwd, rc_fwd), ("inverse", r_inv, rc_inv), ("jacobian", r_jac, rc_jac)): + if (ours != 0) != (theirs != 0): + fail("%s returned %d in the loader and %d in realtime" % (what, ours, theirs)) + +if r_fwd == 0 and rc_fwd == 0: + for n in "xyzabcuvw": + compare("forward %s" % n, getattr(F, n), hal.get_value("paritycheck.fwd-%s" % n)) +if r_inv == 0 and rc_inv == 0: + for j in range(joints): + compare("inverse joint %d" % j, qi[j], hal.get_value("paritycheck.inv-%d" % j)) +if r_jac == 0 and rc_jac == 0: + for j in range(joints): + for a, n in enumerate("xyzabcuvw"): + compare("jacobian [%d][%s]" % (j, n), J[j][a], hal.get_value("paritycheck.jac-%d-%s" % (j, n))) + +kins.kinematicsUserFree(ctx) +halc.hal_exit(comp_id) + +if failures: + sys.exit(1) +print("kins-params: %s type %d agrees" % (module, ktype)) diff --git a/tests/kins-params/checkresult b/tests/kins-params/checkresult new file mode 100755 index 00000000000..d3eba1a4da0 --- /dev/null +++ b/tests/kins-params/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +[ "$(grep -c 'agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ + && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-params/paritycheck.c b/tests/kins-params/paritycheck.c new file mode 100644 index 00000000000..9043df69dac --- /dev/null +++ b/tests/kins-params/paritycheck.c @@ -0,0 +1,148 @@ +/* + * paritycheck: the realtime half of the parameter block parity test. + * + * Loaded after a kinematics module, it evaluates the module through the + * classic entry points once, at load, and publishes the answers on HAL + * pins: the forward pose, the inverse joints and the Jacobian. check.py + * then evaluates the same module through the non-realtime loader, which + * goes through kinsDescribe() and the parameter block, and compares. + * + * Two flows. With frompose=0 the input is a joint set: the pose is the + * forward of it, the joints published are the inverse of that pose, and + * the Jacobian is taken there. With frompose=1 the input is a pose, for + * the parallel machines whose forward wants a seed: the joints are its + * inverse, the forward is run from the pose as seed, and the Jacobian is + * taken there. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + * frompose 0 or 1, as above + * pose up to nine integers, the pose (frompose=1) or the forward + * seed (frompose=0) + * jnt up to sixteen integers, the joint set (frompose=0) or the + * inverse seed (frompose=1) + */ +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 3; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); +static int frompose = 0; +RTAPI_MP_INT(frompose, "1 to take the pose as the input"); +static int pose[EMCMOT_MAX_AXIS] = { 0 }; +RTAPI_MP_ARRAY_INT(pose, EMCMOT_MAX_AXIS, "pose, x y z a b c u v w"); +static int jnt[EMCMOT_MAX_JOINTS] = { 10, 20, 30, 40, 50, 60, 70, 80, 90 }; +RTAPI_MP_ARRAY_INT(jnt, EMCMOT_MAX_JOINTS, "joint values, from joint 0"); + +static int comp_id = -1; + +static struct { + hal_real_t fwd[EMCMOT_MAX_AXIS]; + hal_real_t inv[EMCMOT_MAX_JOINTS]; + hal_real_t jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + hal_sint_t rc_fwd; + hal_sint_t rc_inv; + hal_sint_t rc_jac; +} *pins; + +static const char letter[EMCMOT_MAX_AXIS] = { 'x','y','z','a','b','c','u','v','w' }; + +static double *coord(EmcPose *p, int a) +{ + switch (a) { + 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; + } +} + +int rtapi_app_main(void) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + double q[EMCMOT_MAX_JOINTS], qi[EMCMOT_MAX_JOINTS]; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + EmcPose P, F, seed; + int a, j, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("paritycheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->fwd[a], 0.0, + "paritycheck.fwd-%c", letter[a]); + } + for (j = 0; j < joints; j++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->inv[j], 0.0, + "paritycheck.inv-%d", j); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->jac[j][a], 0.0, + "paritycheck.jac-%d-%c", j, letter[a]); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_fwd, 0, "paritycheck.rc-fwd"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_inv, 0, "paritycheck.rc-inv"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_jac, 0, "paritycheck.rc-jac"); + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { *coord(&seed, a) = pose[a]; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { q[j] = jnt[j]; qi[j] = jnt[j]; } + + // a switchable module's first forward after load restarts from the + // pose it saved, which is nothing yet; take that call here so the one + // measured starts from the seed like the loader's does + F = seed; + kinematicsForward(q, &F, &fflags, &iflags); + fflags = 0; iflags = 0; + + if (frompose) { + P = seed; + hal_set_si32(pins->rc_inv, kinematicsInverse(&P, qi, &iflags, &fflags)); + F = seed; + fflags = 0; iflags = 0; + hal_set_si32(pins->rc_fwd, kinematicsForward(qi, &F, &fflags, &iflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &P, jac, &iflags)); + } else { + F = seed; + hal_set_si32(pins->rc_fwd, kinematicsForward(q, &F, &fflags, &iflags)); + iflags = 0; fflags = 0; + hal_set_si32(pins->rc_inv, kinematicsInverse(&F, qi, &iflags, &fflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &F, jac, &iflags)); + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->fwd[a], *coord(&F, a)); } + for (j = 0; j < joints; j++) { + hal_set_real(pins->inv[j], qi[j]); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->jac[j][a], jac[j][a]); } + } + + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-params/skip b/tests/kins-params/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-params/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh new file mode 100755 index 00000000000..82f5d294e23 --- /dev/null +++ b/tests/kins-params/test.sh @@ -0,0 +1,161 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install paritycheck.c >/dev/null + +# One hal file per module. paritycheck evaluates the module in realtime +# through the classic entry points and publishes the answers; check.py +# evaluates it through the non-realtime loader, kinsDescribe() and the +# parameter block, and compares. Where they disagree the module keeps +# state its table does not declare. +# ONLY= in the environment runs the entries for that module alone +run() { + local loadrt="$1" setp="$2" parms="$3" ktype="$4" + local module coords sparm joints frompose pose jnt hal tok + case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac + module=${loadrt%% *} + coords=""; sparm="" + for tok in $loadrt; do + case "$tok" in + coordinates=*) coords=${tok#coordinates=} ;; + sparm=*) sparm=${tok#sparm=} ;; + esac + done + joints=3; frompose=0; pose="0,0,0,0,0,0,0,0,0"; jnt="10,20,30,40,50,60,70,80,90" + for tok in $parms; do + case "$tok" in + joints=*) joints=${tok#joints=} ;; + frompose=*) frompose=${tok#frompose=} ;; + pose=*) pose=${tok#pose=} ;; + jnt=*) jnt=${tok#jnt=} ;; + esac + done + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s\n' "$loadrt" + printf '%s\n' "$setp" + printf 'loadrt paritycheck %s ktype=%s\n' "$parms" "${ktype:-0}" + # halcmd keeps quotes, so an absent value travels as a dash + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" + } > "$hal" + echo "=== $loadrt type ${ktype:-0}" + halrun -f "$hal" + rm -f "$hal" +} + +# identity, a gantry included +run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20" +run "trivkins coordinates=XYZABCUVW" "" "joints=9" +run "userkins" "" "joints=3 jnt=10,20,30" +run "millturn" "" "joints=4 jnt=10,20,30,40" +run "millturn" "" "joints=4 jnt=10,20,30,40" 1 + +# linear maps and one rotation +run "corexykins" "" "joints=9" +run "rotatekins" "" "joints=9" +run "matrixkins" \ + "setp matrixkins.C_xy 0.02 +setp matrixkins.C_xz -0.01 +setp matrixkins.C_yx 0.03 +setp matrixkins.C_yz 0.015 +setp matrixkins.C_zx -0.02 +setp matrixkins.C_zy 0.01 +setp matrixkins.C_zz 1.001" \ + "joints=9" + +# tables and heads, offsets set so no term drops out +run "maxkins" \ + "setp maxkins.pivot-length 100" \ + "joints=9 jnt=10,20,30,0,15,25,7,0,3" + +run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5" 1 + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" + +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400 +setp xyzacb_trsrn_kins.tool-offset-z 50 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35" 2 + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400 +setp xyzbca_trsrn_kins.tool-offset-z 50 +setp xyzbca_trsrn_kins.pre-rot 0.3 +setp xyzbca_trsrn_kins.primary-angle 20 +setp xyzbca_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35" 1 + +# polar +run "rosekins" "" "joints=3 jnt=10,5,30" + +# arms +run "scarakins" "" "joints=6 jnt=30,40,20,10,0,0" +run "scorbot-kins" "" "joints=5 jnt=40,60,-20,0,0" +run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20" +run "three21kins" "" "joints=6 jnt=15,20,-35,10,70,20" +run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" + +# parallel machines, from a pose the forward can be seeded with +run "tripodkins" \ + "setp tripodkins.Bx 2 +setp tripodkins.Cx 1 +setp tripodkins.Cy 2" \ + "joints=3 frompose=1 pose=1,1,2" +run "lineardeltakins" "" "joints=9 frompose=1 pose=20,30,-200" +run "rotarydeltakins" "" "joints=9 frompose=1 pose=0,0,-12" +run "genhexkins" "setp genhexkins.screw-lead 0" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "genhexkins" "setp genhexkins.screw-lead 5" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "pentakins" "" "joints=5 frompose=1 pose=10,20,0,5,-7" From 7e7493bde3653cb64f0494c3c028dd201ab36d6f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 16:33:19 +1000 Subject: [PATCH 57/58] kinematics: take the tool offset from motion, not from a net A module whose maths needs the tool length read it from a HAL pin the config had to net from motion.tooloffset.z. The offset is controller state, from G43 and the tool table, which motion already holds and only published; the pin was a copy of it, one cycle late, and a missing net gave wrong joints with no error. The parameter block carries the tool, so hand it over directly. Motion calls kinematicsSetTool() whenever the offset changes. It references the symbol weakly, so a module written before the call still loads and keeps its pin. kins_single.c and switchkins.c export it for every module written on the block: after motion has sent anything the tool entry is overwritten with motion's value, and the pin is read only until then, as under halrun with the module alone. A pin left disagreeing with motion for a thousand calls is reported once, since it is a config setting a tool length where the tool table should. The non-realtime loader takes the tool from the caller through kinematicsUserSetTool() where one is given, since a planner knows what a segment runs under better than the machine does, and from motion's pins otherwise as before. tests/kins-tool-offset runs xyzac-trt-kins under motion with nothing on its tool-offset pin and checks that G43 reaches the joints, that connecting the pin the old way changes nothing, and that G49 takes the length out again. tests/kins-params checks that the caller's tool moves the loader's inverse and that handing it back restores realtime's answer. --- docs/src/code/code-notes.adoc | 3 +- docs/src/motion/5-axis-kinematics.adoc | 21 ++-- docs/src/motion/kinematics-conventions.adoc | 34 ++++-- docs/src/motion/switchkins.adoc | 8 +- src/emc/kinematics/kinematics.h | 9 ++ src/emc/kinematics/kins_rt.h | 20 ++++ src/emc/kinematics/kins_single.c | 13 +- src/emc/kinematics/kins_util.c | 36 ++++++ src/emc/kinematics/switchkins.c | 16 ++- .../kinematics_userspace/kinematics_user.c | 29 ++++- .../kinematics_userspace/kinematics_user.h | 15 ++- src/emc/motion/command.c | 8 ++ tests/kins-params/check.py | 29 +++++ tests/kins-tool-offset/README | 7 ++ tests/kins-tool-offset/checkresult | 2 + tests/kins-tool-offset/sim.hal | 20 ++++ tests/kins-tool-offset/test-ui.py | 113 ++++++++++++++++++ tests/kins-tool-offset/test.ini | 112 +++++++++++++++++ tests/kins-tool-offset/test.sh | 2 + tests/kins-tool-offset/tool.tbl | 1 + 20 files changed, 465 insertions(+), 33 deletions(-) create mode 100644 tests/kins-tool-offset/README create mode 100755 tests/kins-tool-offset/checkresult create mode 100644 tests/kins-tool-offset/sim.hal create mode 100755 tests/kins-tool-offset/test-ui.py create mode 100644 tests/kins-tool-offset/test.ini create mode 100755 tests/kins-tool-offset/test.sh create mode 100644 tests/kins-tool-offset/tool.tbl diff --git a/docs/src/code/code-notes.adoc b/docs/src/code/code-notes.adoc index 3874fd37002..064db167757 100644 --- a/docs/src/code/code-notes.adoc +++ b/docs/src/code/code-notes.adoc @@ -1312,8 +1312,7 @@ settings.tool_offset:: + * Used to compute position in various places. * Sent to Motion via the +EMCMOT_SET_OFFSET+ message. - All motion does with the offsets is export them to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+. - FIXME: export these from someplace closer to the tool table (io or interp, probably) and remove the EMCMOT_SET_OFFSET message. + Motion exports the offsets to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+ and hands them to the kinematics module through +kinematicsSetTool()+, for a module whose maths needs the tool length. settings.pockets_max:: Used interchangeably with +CANON_POCKETS_MAX+ (a #defined constant, set to 1000 as of April 2020). diff --git a/docs/src/motion/5-axis-kinematics.adoc b/docs/src/motion/5-axis-kinematics.adoc index 9391f8611a9..1c6e0fe406c 100644 --- a/docs/src/motion/5-axis-kinematics.adoc +++ b/docs/src/motion/5-axis-kinematics.adoc @@ -317,23 +317,17 @@ See the simulation INI files for details of the HAL connections used for the vis === Tool-Length Compensation -In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, LinuxCNC has a variable called "motion.tooloffset.z". If this variable is passed on to the kinematic component (and vismach python script), then the necessary additional Z-offset for a new tool can be accounted for by adding the component statement, for example: +In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, the kinematics accounts for the tool length in effect, for example: image::5-axis-figures/equation__38.png[align="center"] -The required HAL connection (for xyzac-trt) is: +Motion hands the tool offset in effect (G43, G49) to the kinematics module directly, so the module sees the tool from the tool table with no HAL connection. The module's tool-offset pin (xyzac-trt-kins.tool-offset) remains for a configuration that connects it, and is read only until motion has sent an offset; a value set on it that disagrees with the tool table is reported once and not used. -[source,hal] ----- -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset ----- - -where: +Motion also publishes the offset on the HAL pin "motion.tooloffset.z", which is what a vismach python script reads to draw the tool: +[source,hal] ---- -:tool-offset ---------------- signal name -motion.tooloffset.z --------- output HAL pin from LinuxCNC motion module -xyzac-trt-kins.tool-offset -- input HAL pin to xyzac-trt-kins +net :tool-offset motion.tooloffset.z xyzac-trt-gui.tool-offset ---- == Custom Kinematics Components @@ -383,17 +377,18 @@ KINEMATICS = kinsname where "kinsname" is the name of your kins program. Additional HAL pins may be created by the module for variable configuration items -such as the D~x~, D~y~, D~z~, tool-offset used in the xyzac-trt kinematics module. +such as the D~x~, D~y~, D~z~ used in the xyzac-trt kinematics module. These pins can be connected to a signal for dynamic control or set once with HAL connections like: [source,hal] ---- # set offset parameters -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset setp xyzac-trt-kins.y-offset 0 setp xyzac-trt-kins.z-offset 20 ---- +The tool length is not among them: motion hands it to the module from the tool table. + == Figures .Table tilting/rotating configuration diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 72858070537..4aadaede16c 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -520,15 +520,26 @@ module with one type describes itself in a `kins_module` and links In realtime it makes one HAL pin per table entry, copies the pins into the block before every call and the outputs back after it, and supplies the classic entry points, `kinematicsForward()` and the rest, so that motion sees no -difference. Outside realtime a module exports `kinsDescribe()`, which hands a -caller its table and the ops of each type; the caller fills a block from -wherever it likes and asks the same functions through `kinsOpsForward()`, -`kinsOpsInverse()`, `kinsOpsJacobian()` and the frame calls, with the same -defaults applied, so both sides get the same answers. The non-realtime loader -in `kinematics_userspace/` binds the pins of the running module by the table's -names and takes the tool from motion's own offset pins, and says once when the -module's tool pin disagrees with them, which is a config that lost the tool on -the way. `kinslimits` is built on it. +difference. It also exports `kinematicsSetTool()`, through which motion hands +the module the tool offset in effect whenever that changes, so the tool comes +from the tool table and not from a net the config had to remember. A table +entry flagged as the tool is read from its pin only until motion has sent +anything; after that the pin is overwritten with motion's value, and the shared +code says once if the pin is left disagreeing with it, which is a config +setting a tool length where the tool table should. Motion references the call +weakly, so a module written before it still loads and keeps its pin. + +Outside realtime a module exports `kinsDescribe()`, which hands a caller its +table and the ops of each type; the caller fills a block from wherever it likes +and asks the same functions through `kinsOpsForward()`, `kinsOpsInverse()`, +`kinsOpsJacobian()` and the frame calls, with the same defaults applied, so +both sides get the same answers. The non-realtime loader in +`kinematics_userspace/` binds the pins of the running module by the table's +names. Its tool is the caller's where the caller gives one through +`kinematicsUserSetTool()`, since a planner knows what a segment runs under +better than the machine does; otherwise it is motion's, from motion's own +offset pins, and the loader says once when the module's tool pin disagrees with +them. `kinslimits` is built on it. A module that does not provide the form keeps working as it did. It just cannot be evaluated outside realtime, which the loader reports. @@ -537,8 +548,9 @@ be evaluated outside realtime, which the loader reports. The kinematics type is in the block, so a caller evaluating a program that switches type puts the type each block will run under in its own block, and -nothing is switched globally. The tool is in the block, from motion. The joint -map is in the block, from `coordinates=`. Nothing else the maths needs exists, +nothing is switched globally. The tool is in the block, from motion in realtime +and from the caller outside it. The joint map is in the block, from +`coordinates=`. Nothing else the maths needs exists, and a module that finds it needs something else has found a parameter it should declare. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 989645ba58b..02bc8a672c0 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -551,7 +551,13 @@ hal_ready() after it. A module built this way also exports kinsDescribe(), through which a copy of it loaded outside realtime learns its table and the maths of -each kinstype; the non-realtime loader and kinslimits use it. +each kinstype; the non-realtime loader and kinslimits use it. It +exports kinematicsSetTool() as well, through which motion hands it +the tool offset in effect whenever that changes. A table entry +flagged as the tool is overwritten with it, and the entry's pin only +matters until motion has sent anything, so a config need not net +motion.tooloffset.z to the module. A kinstype registered the older +way reads its own pins and is not affected. === Module main program diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 9162e8e3335..3085c5fcfa5 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -640,6 +640,15 @@ extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); + +/* The tool offset motion applies, handed to the module. Motion calls this + whenever the offset changes (G43, G49) and references it weakly, so a + module that does not export it still loads and keeps reading whatever + tool pin it has. kins_single.c and switchkins.c export it for every + module written on the parameter block: the tool then comes from the tool + table through motion, and the module's tool pin, where it has one, is + read only until motion has spoken. */ +extern int kinematicsSetTool(const EmcPose *tool); //NOTE: switchable kinematics may require Interp::Synch // before/after invoking kinematicsSwitch() // A convenient command to synch is: M66 E0 L0 diff --git a/src/emc/kinematics/kins_rt.h b/src/emc/kinematics/kins_rt.h index 96d7309c739..78d0a61ef7d 100644 --- a/src/emc/kinematics/kins_rt.h +++ b/src/emc/kinematics/kins_rt.h @@ -42,6 +42,26 @@ extern void kinsParamsPinsWrite(const kins_pin_ref *pins, const kins_param_desc *params, int nparams, const kins_scratch *s); +/* Where the RT block's tool comes from. kinematicsSetTool() records what + motion sends in one of these; kinsToolSourceApply() writes it into a + block after the pins have been read, over the tool entry, once motion + has sent anything. Until then the tool entry's pin is all there is, as + under halrun with the module alone. A config that still nets the tool + to the module's pin loses nothing; one that sets that pin to something + else is told, once, after the two have disagreed for a thousand calls, + since the pin lags the send by a cycle. */ +typedef struct { + EmcPose tool; + int have; /* motion has sent a tool */ + int disagreeing; /* consecutive calls with the pin elsewhere */ + int warned; +} kins_tool_source; + +extern void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool); +extern void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p); + /* A module with one kinematics type defines this, describing itself, and links kins_single.c, which supplies kinematicsForward() and the rest from it. ops[0] is the maths; the other entries are ignored. */ diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 58f914076d5..ee7cc4b29ed 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -20,6 +20,7 @@ static kins_params rt_params; static kins_scratch rt_scratch; static kins_pin_ref *pins; +static kins_tool_source tool_source; static int inited; static KINEMATICS_TYPE reported_type = KINEMATICS_BOTH; @@ -28,11 +29,13 @@ static const kins_ops *ops(void) return inited ? kins_module.ops[0] : NULL; } -// the block sees the pins as they are now +// the block sees the pins as they are now, and the tool motion sent static void read_pins(void) { kinsParamsPinsRead(pins, kins_module.params, kins_module.nparams, &rt_params); + kinsToolSourceApply(&tool_source, kins_module.halprefix, + kins_module.params, kins_module.nparams, &rt_params); } static void write_pins(void) @@ -117,6 +120,13 @@ int kinematicsJacobian(const double *joint, return kinsOpsJacobian(ops(), &rt_params, &rt_scratch, joint, pos, jac, iflags); } +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} + KINEMATICS_TYPE kinematicsType(void) { return reported_type; @@ -150,6 +160,7 @@ EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 6c285f19c91..6ff782ee427 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1587,3 +1587,39 @@ void kinsParamsPinsWrite(const kins_pin_ref *pins, } } } // kinsParamsPinsWrite() + +void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool) +{ + if (!src || !tool) { return; } + src->tool = *tool; + src->have = 1; +} // kinsToolSourceSet() + +void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!src || !p || !src->have) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double diff; + if (!d->tool || d->dir == KINS_OUT) { continue; } + diff = p->geometry[i] - src->tool.tran.z; + if (diff > 1e-9 || diff < -1e-9) { + if (src->disagreeing < 1000) { + src->disagreeing++; + } else if (!src->warned) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s.%s disagrees with the tool offset motion applies;" + " motion's is used, the pin is not needed\n", + prefix ? prefix : "kins", d->name); + src->warned = 1; + } + } else { + src->disagreeing = 0; + } + p->geometry[i] = src->tool.tran.z; + } + p->tool = src->tool; +} // kinsToolSourceApply() diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 1c895bedb78..50cb8a785f8 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -55,6 +55,7 @@ static const kins_ops *kops[SWITCHKINS_MAX_TYPES] = {NULL}; static kins_params rt_params; static kins_scratch rt_scratch[SWITCHKINS_MAX_TYPES]; static kins_pin_ref *pins; +static kins_tool_source tool_source; static int inited; // types provided, counted in rtapi_app_main() once they are all in @@ -108,13 +109,25 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() -// the block sees the pins as they are now, and the type asked for +// the block sees the pins as they are now, the tool motion sent, and +// the type asked for static void read_block(int ktype) { rt_params.ktype = ktype; kinsParamsPinsRead(pins, kp.params, kp.nparams, &rt_params); + kinsToolSourceApply(&tool_source, kp.halprefix, kp.params, kp.nparams, + &rt_params); } +// the tool from motion, for the types written on the block; a type +// provided the older way reads its own pins and does not see it +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} // kinematicsSetTool() + static void write_block(int ktype) { kinsParamsPinsWrite(pins, kp.params, kp.nparams, &rt_scratch[ktype]); @@ -507,6 +520,7 @@ EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 637bc2de12f..82e59f7a4e9 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -6,7 +6,9 @@ * kinsDescribe(), and evaluates its kinematics through the parameter * block (see kinematics.h). The block is filled from HAL: one input pin * of the caller's component per table entry, connected to the signal the - * RT instance's pin reads, so the values are the live ones; and the tool + * RT instance's pin reads, so the values are the live ones. The tool is + * the caller's where it has given one, since a planner knows what a + * segment runs under better than the machine does; otherwise it comes * from motion's own tooloffset pins where motion is loaded, so that the * tool the module sees is the one motion has, whether or not the config * netted it to the module's pin. @@ -57,6 +59,8 @@ struct KinematicsUserContext { int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ int tool_param; /* the table's tool entry, -1 if none */ int warned_tool; + EmcPose caller_tool; /* from kinematicsUserSetTool() */ + int have_caller_tool; double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ }; @@ -232,7 +236,8 @@ static int bind_all(KinematicsUserContext *ctx) return 0; } -/* The block sees the pins as they are now. */ +/* The block sees the pins as they are now, and the tool of whoever + knows it best: the caller, then motion, then the module's own pin. */ static void refresh(KinematicsUserContext *ctx) { int i; @@ -248,6 +253,14 @@ static void refresh(KinematicsUserContext *ctx) ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; } + if (ctx->have_caller_tool) { + ctx->params.tool = ctx->caller_tool; + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = ctx->caller_tool.tran.z; + } + return; + } + for (i = 0; i < AXIS_COUNT; i++) { int c = ctx->cell_of_tool[i]; tool[i] = 0.0; @@ -423,6 +436,18 @@ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx) return ctx->info.ntypes; } +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (tool) { + ctx->caller_tool = *tool; + ctx->have_caller_tool = 1; + } else { + ctx->have_caller_tool = 0; + } + return 0; +} + int kinematicsUserInverse(KinematicsUserContext* ctx, const EmcPose* world, double* joints) diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 3d1e8c2bf8f..e3090a34bd5 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -9,8 +9,9 @@ * The kinematics module is loaded into this process and evaluated through * its parameter block form (see kinematics.h). The block is filled from * input pins belonging to the caller's HAL component, connected to the - * same signals the running RT instance reads, and from motion's tool - * offset pins where motion is loaded, so the maths runs on live values. + * same signals the running RT instance reads, so the maths runs on live + * values; the tool is the caller's where it gives one, and motion's + * otherwise, from motion's tool offset pins where motion is loaded. * * Author: LinuxCNC * License: GPL Version 2 @@ -87,6 +88,16 @@ int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype); */ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); +/** + * The tool offset to evaluate with: what the caller knows the segment + * runs under, from canon or the tool table, rather than the offset the + * machine happens to have now. It stands until replaced, or until NULL + * puts the context back to taking the tool from motion. + * + * @return 0, or -1 for an RT-only context + */ +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool); + /** * Perform inverse kinematics (world coords -> joint positions) * diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 22b51ac533f..cf0079ef6ce 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -70,6 +70,11 @@ #include "homing.h" #include "axis.h" +// the kinematics module takes the tool offset from here when it can; a +// module written before the call exports no such symbol, and the weak +// reference leaves it NULL rather than refusing to load motion +#pragma weak kinematicsSetTool + #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -1991,6 +1996,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); emcmotStatus->tool_offset = emcmotCommand->tool_offset; + if (kinematicsSetTool) { + kinematicsSetTool(&emcmotStatus->tool_offset); + } break; case EMCMOT_SET_AXIS_POSITION_LIMITS: diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index b6f7758f892..265fb606e8c 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -128,6 +128,35 @@ def compare(what, ours, theirs): for a, n in enumerate("xyzabcuvw"): compare("jacobian [%d][%s]" % (j, n), J[j][a], hal.get_value("paritycheck.jac-%d-%s" % (j, n))) +# the caller's tool wins over the module's pin: for a module with a tool +# entry, a length of the caller's must move the inverse, and handing the +# tool back to HAL must return it to what realtime found +kins.kinematicsUserSetTool.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose)] +tool_pin = None +for name in ("tool-offset", "tool-offset-z"): + try: + hal.get_value("%s.%s" % (module, name)) + tool_pin = name + except RuntimeError: + pass +if r_inv == 0 and rc_inv == 0 and tool_pin: + P = pose_of(pose_in) if frompose else F + T = pose_of([0.0] * AXES) + T.z = hal.get_value("%s.%s" % (module, tool_pin)) + 10.0 + kins.kinematicsUserSetTool(ctx, ctypes.byref(T)) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the caller's tool") + elif all(close(qt[j], qi[j]) for j in range(joints)): + fail("the caller's tool did not move the inverse") + kins.kinematicsUserSetTool(ctx, None) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the tool handed back") + else: + for j in range(joints): + compare("inverse joint %d after the tool is handed back" % j, qt[j], qi[j]) + kins.kinematicsUserFree(ctx) halc.hal_exit(comp_id) diff --git a/tests/kins-tool-offset/README b/tests/kins-tool-offset/README new file mode 100644 index 00000000000..db4bd42a726 --- /dev/null +++ b/tests/kins-tool-offset/README @@ -0,0 +1,7 @@ +The kinematics module takes the tool offset from motion, not from a net. + +Runs xyzac-trt-kins under motion with nothing connected to its tool-offset +pin, applies a tool length through G43, and checks that the joints move as +the tool length requires. Then connects motion.tooloffset.z to the pin the +old way and checks that nothing changes, and that G49 takes the length back +out through motion alone. diff --git a/tests/kins-tool-offset/checkresult b/tests/kins-tool-offset/checkresult new file mode 100755 index 00000000000..24dc9aa53e3 --- /dev/null +++ b/tests/kins-tool-offset/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +exit 0 # test failure is indicated by test.sh exit value diff --git a/tests/kins-tool-offset/sim.hal b/tests/kins-tool-offset/sim.hal new file mode 100644 index 00000000000..81a0df64444 --- /dev/null +++ b/tests/kins-tool-offset/sim.hal @@ -0,0 +1,20 @@ +# the module under test, with nothing on its tool-offset pin +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +# offsets, so that the tool length reaches the joints through a rotation +setp xyzac-trt-kins.y-offset 20 +setp xyzac-trt-kins.z-offset 10 + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-tool-offset/test-ui.py b/tests/kins-tool-offset/test-ui.py new file mode 100755 index 00000000000..dbd174caa66 --- /dev/null +++ b/tests/kins-tool-offset/test-ui.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# The kinematics module takes the tool offset from motion. +# +# xyzac-trt-kins runs with nothing connected to its tool-offset pin. A +# tool length applied with G43 must still reach the joints, since motion +# hands the offset to the module; connecting motion.tooloffset.z to the +# pin afterwards, the old way, must change nothing; and G49 must take the +# length back out again through motion alone. + +import linuxcnc +import hal +import subprocess +import sys +import os +import time + +TOOL_LENGTH = 25.0 +POSE = "G0 X10 Y20 Z30 A30 C45" +AWAY = "G0 X0 Y0 Z0 A0 C0" + +c = linuxcnc.command() +s = linuxcnc.stat() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(30) + +def joints(): + # the commanded joint positions once the move has settled: in position, + # nothing queued, and the same answer twice in a row, since the in + # position flag can go up a cycle before the last increment lands + deadline = time.time() + 30 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(5)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.1) + error("timed out waiting for the move") + return last + +def same(a, b, tol=1e-6): + return all(abs(x - y) <= tol for x, y in zip(a, b)) + +def show(what, j): + print("%-28s %s" % (what, " ".join("%.6f" % v for v in j))) + +# no tool: the pose with nothing applied +mdi("G49", POSE) +base = joints() +show("G49", base) + +# tool applied through motion, the pin still at its default +mdi("G43 H1", AWAY, POSE) +with_tool = joints() +show("G43 H1, pin unconnected", with_tool) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if pin != 0.0: + error("the tool-offset pin reads %g with nothing connected" % pin) +if same(base, with_tool): + error("the tool length did not reach the joints") + +# the table on rotaries at A30 C45: the tool length moves Y and Z joints, +# by a known amount, since the pivot geometry is the module's alone +tool_z = hal.get_value("motion.tooloffset.z") +if abs(tool_z - TOOL_LENGTH) > 1e-9: + error("motion.tooloffset.z is %g, expected %g" % (tool_z, TOOL_LENGTH)) +if abs(with_tool[0] - base[0]) > 1e-6: + error("the tool length moved joint 0, which the A rotation does not touch") + +# the old connection: nothing may change +subprocess.check_call(["halcmd", "net", ":tool-offset", + "motion.tooloffset.z", "xyzac-trt-kins.tool-offset"]) +mdi(AWAY, POSE) +with_net = joints() +show("G43 H1, pin connected", with_net) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if abs(pin - TOOL_LENGTH) > 1e-9: + error("the connected tool-offset pin reads %g" % pin) +if not same(with_tool, with_net): + error("connecting the pin changed the joints") + +# and back out, through motion, with the pin connected +mdi("G49", AWAY, POSE) +without = joints() +show("G49, pin connected", without) +if not same(base, without): + error("G49 did not take the tool length back out") + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-tool-offset/test.ini b/tests/kins-tool-offset/test.ini new file mode 100644 index 00000000000..bb7839671ed --- /dev/null +++ b/tests/kins-tool-offset/test.ini @@ -0,0 +1,112 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0x0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 20 +MAX_LINEAR_VELOCITY = 200 +MAX_LINEAR_ACCELERATION = 2000 +NO_FORCE_HOMING = 1 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[KINS] +KINEMATICS = xyzac-trt-kins +JOINTS = 5 + +[AXIS_X] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Y] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Z] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_A] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -100 +MAX_LIMIT = 100 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +HOME_SEQUENCE = 0 diff --git a/tests/kins-tool-offset/test.sh b/tests/kins-tool-offset/test.sh new file mode 100755 index 00000000000..a31b772a81c --- /dev/null +++ b/tests/kins-tool-offset/test.sh @@ -0,0 +1,2 @@ +#!/bin/bash -e +linuxcnc -r test.ini diff --git a/tests/kins-tool-offset/tool.tbl b/tests/kins-tool-offset/tool.tbl new file mode 100644 index 00000000000..acb961918d9 --- /dev/null +++ b/tests/kins-tool-offset/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 ;the tool with a length From c264d2c0a807fd504cc2970d2f65c0a50f820fab Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 18:46:45 +1000 Subject: [PATCH 58/58] tests: put the nutating head kinematics next to the tilted work plane maths The two trsrn configs carry their orientation maths in python, remap_funcs_twp.py, written apart from the kinematics modules. Nothing had compared the two. tests/kins-twp loads each module in realtime with a small component that answers frame and inverse requests over HAL pins, and drives it from python holding the twp functions. Over a grid of primary, secondary and table angles the module's tool frame in machine coordinates equals the python transformation matrix at zero virtual rotation to 1e-9, both machines. For a set of requested tool axes, with the table held as the remap holds it, the joint pairs the module finds are the python's candidate pairs, and the spin it reports for the python's horizontal tool x is the python's virtual rotation. With nothing held the module may turn the table, which the python never does, so there each side is judged by the other's maths: a module solution reaches the axis through the python head matrix composed with the module's table frame, and reaches the full frame when tool x is given. --- tests/kins-twp/README | 9 ++ tests/kins-twp/check.py | 273 ++++++++++++++++++++++++++++++++++ tests/kins-twp/checkresult | 3 + tests/kins-twp/skip | 4 + tests/kins-twp/test.sh | 30 ++++ tests/kins-twp/twp-xyzacb.ini | 16 ++ tests/kins-twp/twp-xyzbca.ini | 16 ++ tests/kins-twp/twpcheck.c | 165 ++++++++++++++++++++ 8 files changed, 516 insertions(+) create mode 100644 tests/kins-twp/README create mode 100755 tests/kins-twp/check.py create mode 100755 tests/kins-twp/checkresult create mode 100755 tests/kins-twp/skip create mode 100755 tests/kins-twp/test.sh create mode 100644 tests/kins-twp/twp-xyzacb.ini create mode 100644 tests/kins-twp/twp-xyzbca.ini create mode 100644 tests/kins-twp/twpcheck.c diff --git a/tests/kins-twp/README b/tests/kins-twp/README new file mode 100644 index 00000000000..b9b3b7275fa --- /dev/null +++ b/tests/kins-twp/README @@ -0,0 +1,9 @@ +The C kinematics against the tilted work plane maths. + +The two nutating-head configs carry their orientation maths in python, +remap_funcs_twp.py, written independently of the kinematics modules. +This test loads each module in realtime and puts its tool frame next to +the python transformation matrix over a grid of head angles, and its +tool frame inverse next to the python candidate joint angles and virtual +rotation for a set of requested tool axes. Where they disagree, one of +the two has the sign or the order of a rotation wrong. diff --git a/tests/kins-twp/check.py b/tests/kins-twp/check.py new file mode 100755 index 00000000000..2b8d52dd683 --- /dev/null +++ b/tests/kins-twp/check.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python3 +# The python half of the tilted work plane cross-check. +# +# Imports the machine's remap_funcs_twp.py, the maths the tilted work +# plane remap orients the head with, and drives twpcheck, loaded after +# the kinematics module, to get the module's answers to the same +# questions. Two comparisons: +# +# frames over a grid of primary angle, secondary angle, virtual +# rotation and table angle, the module's tool frame in +# machine coordinates against the python transformation +# matrix Rp * Rs * Rtc +# inverse for a set of requested tool axes, with the table held as +# the remap holds it, the joint angle pairs the module's +# kinematicsToolFrameInverse() finds against the pairs the +# python candidate search keeps, and the spin about the tool +# the module reports for the python's horizontal tool x +# against the python's own virtual rotation; then with nothing +# held, where the module may turn the table, each side judged +# by the other's maths +# +# Usage: check.py MACHINE CONFIGDIR INIFILE +# MACHINE is xyzacb or xyzbca; CONFIGDIR holds remap_funcs_twp.py; +# INIFILE is what that file reads its letters and limits from. + +import os +import sys +import time +from math import radians, degrees, pi, sin, cos, atan2 + +import numpy as np +import hal + +machine, cfgdir, inifile = sys.argv[1:4] +os.environ["INI_FILE_NAME"] = inifile +sys.path.insert(0, cfgdir) +import remap_funcs_twp as twp + +# joint numbers: the table, the secondary and the primary rotary +TABLE, SECONDARY, PRIMARY = {"xyzacb": (3, 4, 5), "xyzbca": (4, 3, 5)}[machine] +PREROT = "%s_trsrn_kins.pre-rot" % machine +TOL = 1e-9 +ANGLE_TOL = 1e-6 # degrees + +failures = 0 +def fail(what): + global failures + failures += 1 + print("kins-twp: FAIL %s: %s" % (machine, what)) + +class Log: + def debug(self, *a, **k): pass + def error(self, *a, **k): print("kins-twp: python error:", a[0] % tuple(a[1:]) if len(a) > 1 else a[0]) +log = Log() + +# ---- driving twpcheck + +request = 0 +def ask(j, axis=None, xdir=None, held=0): + """set the joints and the request, wait for the answer""" + global request + for i, v in enumerate(j): + hal.set_p("twpcheck.j-%d" % i, str(v)) + hal.set_p("twpcheck.held", str(held)) + for i, c in enumerate("xyz"): + hal.set_p("twpcheck.axis-%s" % c, str(axis[i] if axis is not None else 0.0)) + hal.set_p("twpcheck.xdir-%s" % c, str(xdir[i] if xdir is not None else 0.0)) + hal.set_p("twpcheck.have-x", "1" if xdir is not None else "0") + request += 1 + hal.set_p("twpcheck.request", str(request)) + deadline = time.time() + 5 + while hal.get_value("twpcheck.done") != request: + if time.time() > deadline: + print("kins-twp: FAIL twpcheck did not answer") + sys.exit(1) + time.sleep(0.002) + +def read_matrix(name): + return np.array([[hal.get_value("twpcheck.%s-%d%d" % (name, r, c)) for c in range(3)] + for r in range(3)]) + +def read_solutions(): + n = hal.get_value("twpcheck.nsol") + sols = [] + for k in range(max(n, 0)): + sols.append(([hal.get_value("twpcheck.sol-%d-%d" % (k, i)) for i in range(6)], + hal.get_value("twpcheck.spin-%d" % k), + hal.get_value("twpcheck.free-%d" % k))) + return n, sols + +def joints_at(table, secondary, primary): + j = [10.0, 20.0, 30.0, 0.0, 0.0, 0.0] + j[TABLE], j[SECONDARY], j[PRIMARY] = table, secondary, primary + return j + +# ---- the python's answers + +def py_matrix(primary_deg, secondary_deg, tc): + m = twp.kins_calc_transformation_matrix(radians(primary_deg), radians(secondary_deg), tc, + np.asmatrix(np.identity(4)), 'inv') + return np.array(m)[:3, :3] + +def py_pairs(z): + """the (primary, secondary) pairs in degrees the remap would keep for a + tool axis, following remap.py: every combination of the candidate + lists, kept where it reaches the axis""" + t1, t2 = twp.kins_calc_possible_joint_angles(log, np.array(z), None) + if t1 is None or t2 is None: + return [] + pairs = [] + for a in set(t1): + for b in set(t2): + m = py_matrix(degrees(a), degrees(b), 0.0) + if np.allclose(m[:, 2], z, atol=1e-6): + pairs.append((degrees(a), degrees(b))) + return pairs + +def same_angle(a, b): + d = (a - b + 180.0) % 360.0 - 180.0 + return abs(d) < ANGLE_TOL + +def same_pair(p, q): + return same_angle(p[0], q[0]) and same_angle(p[1], q[1]) + +def fmt(m): + return np.array2string(m, precision=6, suppress_small=True) + +# ---- frames +# +# The module's frame is the head's rotation from its joints alone, so it +# is compared with the python matrix at zero virtual rotation; whether the +# frame should carry the virtual rotation too is a convention question the +# test does not settle. + +frames = 0 +hal.set_p(PREROT, "0") +for table in (0.0, 20.0): + for primary in (0.0, 30.0, -25.0, 90.0, 180.0, -135.0): + for secondary in (0.0, 30.0, -25.0, 90.0, -90.0, 180.0): + ask(joints_at(table, secondary, primary)) + if hal.get_value("twpcheck.frame-rc") != 0: + fail("no frame at primary %g secondary %g" % (primary, secondary)) + continue + tool = read_matrix("tool") + want = py_matrix(primary, secondary, 0.0) + frames += 1 + if not np.allclose(tool, want, atol=TOL): + fail("tool frame differs at primary %g secondary %g table %g\n module:\n%s\n python:\n%s" + % (primary, secondary, table, fmt(tool), fmt(want))) + +# ---- inverse, the table held +# +# The remap holds the table and orients the head, so ask the module the +# same: the joint pairs must then be the python's, and the spin about the +# tool for the python's horizontal tool x must be the python's virtual +# rotation. + +def rz(a): + return np.array([[cos(a), -sin(a), 0.0], [sin(a), cos(a), 0.0], [0.0, 0.0, 1.0]]) + +def frames_at(j): + ask(j) + return read_matrix("work"), read_matrix("tool") + +def in_work(work, tool): + return work.T @ tool + +HOLD_TABLE = 1 << TABLE +REQUESTS = ((30.0, 30.0), (-25.0, 60.0), (120.0, -45.0), (180.0, 90.0), + (0.0, 0.0), (90.0, 135.0), (45.0, 170.0), (-100.0, -20.0)) + +requests = 0 +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + if not any(same_pair(p, (primary, secondary)) for p in pairs): + fail("the python does not find the pair (%g, %g) the axis was made from" % (primary, secondary)) + + ask(seed, axis=z, held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("with the table held, the module finds no solution for " + where) + continue + found = [(s[0][PRIMARY], s[0][SECONDARY]) for s in sols] + for s in sols: + j, spin, free = s + if abs(j[TABLE] - seed[TABLE]) > 1e-12: + fail("the held table moved for " + where) + if free != 0 and (primary, secondary) != (0.0, 0.0): + fail("with the table held a solution is still a family for " + where) + for p in pairs: + if not any(same_pair(p, f) for f in found): + fail("python pair (%.6f, %.6f) not among the module's %s for %s" + % (p[0], p[1], ["(%.6f, %.6f)" % f for f in found], where)) + for f in found: + if not any(same_pair(p, f) for p in pairs): + fail("module pair (%.6f, %.6f) not among the python's %s for %s" + % (f[0], f[1], ["(%.6f, %.6f)" % p for p in pairs], where)) + + # tool x as the python's virtual rotation places it, horizontal: the + # module, holding the table, must answer the same pair with that spin + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + if abs(full[2, 0]) > 1e-9: + fail("python virtual rotation %g leaves tool x off horizontal for pair (%.6f, %.6f)" % (tc, p[0], p[1])) + ask(seed, axis=z, xdir=full[:, 0], held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + match = [s for s in sols if same_pair((s[0][PRIMARY], s[0][SECONDARY]), p)] + if not match: + fail("with tool x given and the table held, pair (%.6f, %.6f) is gone from the module's answers" % p) + continue + spin = match[0][1] + if abs((spin - tc + pi) % (2 * pi) - pi) > 1e-6: + fail("module spin %.9f and python virtual rotation %.9f differ for pair (%.6f, %.6f)" + % (spin, tc, p[0], p[1])) + +# ---- inverse, nothing held +# +# The module may now turn the table, since it turns the tool against the +# work as surely as the head does, and reports one member of the family +# that results. Not the python's answer, so each is judged by the other's +# maths: a module solution must reach the axis through the python head +# matrix composed with the module's table frame, and with tool x given it +# must reach the whole frame. + +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + + ask(seed, axis=z) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution for " + where) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + if not np.allclose(tool, py_matrix(j[PRIMARY], j[SECONDARY], 0.0), atol=TOL): + fail("module frame at its own solution differs from the python head matrix for " + where) + if not np.allclose(in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0))[:, 2], z, atol=1e-6): + fail("module solution %s does not reach %s by the python head matrix" % (fmt(np.array(j)), where)) + for i in (0, 1, 2): + if abs(j[i] - seed[i]) > 1e-9: + fail("solution moved linear joint %d for %s" % (i, where)) + + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + ask(seed, axis=z, xdir=full[:, 0]) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution with tool x given for pair (%.6f, %.6f)" % p) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + achieved = in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0) @ rz(spin)) + if not np.allclose(achieved, full, atol=1e-6): + fail("module solution %s spin %.6f does not reach the python frame for pair (%.6f, %.6f)\n achieved:\n%s\n wanted:\n%s" + % (fmt(np.array(j)), spin, p[0], p[1], fmt(achieved), fmt(full))) + +if failures: + sys.exit(1) +print("kins-twp: %s agrees, %d frames, %d requests" % (machine, frames, requests)) diff --git a/tests/kins-twp/checkresult b/tests/kins-twp/checkresult new file mode 100755 index 00000000000..011ea9232ad --- /dev/null +++ b/tests/kins-twp/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +[ "$(grep -c 'kins-twp: .* agrees' "$1")" = 2 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-twp/skip b/tests/kins-twp/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-twp/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-twp/test.sh b/tests/kins-twp/test.sh new file mode 100755 index 00000000000..86b77f6ff44 --- /dev/null +++ b/tests/kins-twp/test.sh @@ -0,0 +1,30 @@ +#!/bin/bash +set -e + +# RIP layout: $HEADERS is $TOPDIR/include +TOPDIR=$(dirname "$HEADERS") +CONFIGS=$TOPDIR/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating + +${SUDO} halcompile --install twpcheck.c >/dev/null + +# One hal file per machine. twpcheck answers frame and inverse requests +# from check.py over HAL pins; check.py holds the python maths. +run() { + local machine=$1 hal + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s_trsrn\n' "$machine" + printf 'setp %s_trsrn_kins.nut-angle 45\n' "$machine" + printf 'loadrt twpcheck joints=6 ktype=1\n' + printf 'loadrt threads name1=t1 period1=1000000\n' + printf 'addf twpcheck t1\n' + printf 'start\n' + printf 'loadusr -w python3 check.py %s %s/%s-trsrn_twp %s/twp-%s.ini\n' \ + "$machine" "$CONFIGS" "$machine" "$PWD" "$machine" + } > "$hal" + echo "=== $machine" + halrun -f "$hal" + rm -f "$hal" +} + +run xyzacb +run xyzbca diff --git a/tests/kins-twp/twp-xyzacb.ini b/tests/kins-twp/twp-xyzacb.ini new file mode 100644 index 00000000000..7d276d86bf8 --- /dev/null +++ b/tests/kins-twp/twp-xyzacb.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzacb_trsrn + +[TWP] +PRIMARY = C +SECONDARY = B + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_B] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twp-xyzbca.ini b/tests/kins-twp/twp-xyzbca.ini new file mode 100644 index 00000000000..b6bfd198e60 --- /dev/null +++ b/tests/kins-twp/twp-xyzbca.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzbca_trsrn + +[TWP] +PRIMARY = C +SECONDARY = A + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_A] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twpcheck.c b/tests/kins-twp/twpcheck.c new file mode 100644 index 00000000000..90a86025569 --- /dev/null +++ b/tests/kins-twp/twpcheck.c @@ -0,0 +1,165 @@ +/* + * twpcheck: the realtime half of the tilted work plane cross-check. + * + * Loaded after a kinematics module, it answers requests made over HAL + * pins: for the joint values on its inputs it reports the module's tool + * frame and work frame, and for the tool axis (and optionally tool x) + * on its inputs it reports what kinematicsToolFrameInverse() finds, the + * joint sets and the spin about the tool each needs, with the joints + * named on the held pin kept where they are. check.py drives it and + * holds the python maths the answers are compared with. + * + * A request is made by raising the request pin; done follows it when + * the answers are on the pins. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + */ +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 6; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); + +static int comp_id = -1; + +#define NSOL TOOL_FRAME_MAX_SOLUTIONS + +static struct { + hal_real_t j[EMCMOT_MAX_JOINTS]; + hal_real_t axis[3]; + hal_real_t xdir[3]; + hal_bool_t have_x; + hal_uint_t held; /* bit per joint the inverse may not move */ + hal_uint_t request; + hal_uint_t done; + hal_real_t tool[3][3]; /* [row][column], columns are the frame's axes */ + hal_real_t work[3][3]; + hal_sint_t frame_rc; + hal_sint_t nsol; + hal_real_t sol[NSOL][EMCMOT_MAX_JOINTS]; + hal_real_t spin[NSOL]; + hal_sint_t free[NSOL]; +} *pins; + +static void publish(hal_real_t out[3][3], const PmRotationMatrix *m) +{ + hal_set_real(out[0][0], m->x.x); hal_set_real(out[0][1], m->y.x); hal_set_real(out[0][2], m->z.x); + hal_set_real(out[1][0], m->x.y); hal_set_real(out[1][1], m->y.y); hal_set_real(out[1][2], m->z.y); + hal_set_real(out[2][0], m->x.z); hal_set_real(out[2][1], m->y.z); hal_set_real(out[2][2], m->z.z); +} + +static void update(void *arg, long period) +{ + KINEMATICS_FORWARD_FLAGS ff = 0; + PmRotationMatrix tool, work; + PmCartesian axis, xdir; + double j[EMCMOT_MAX_JOINTS]; + double sols[NSOL * EMCMOT_MAX_JOINTS]; /* rows of joints doubles, packed */ + double spin[NSOL]; + int freed[NSOL]; + int i, k, n, rc; + (void)arg; + (void)period; + + if (hal_get_ui32(pins->request) == hal_get_ui32(pins->done)) { return; } + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = i < joints ? hal_get_real(pins->j[i]) : 0.0; + } + + rc = kinematicsToolFrame(j, &tool, &ff); + if (!rc) { rc = kinematicsWorkFrame(j, &work, &ff); } + hal_set_si32(pins->frame_rc, rc); + if (!rc) { + publish(pins->tool, &tool); + publish(pins->work, &work); + } + + axis.x = hal_get_real(pins->axis[0]); + axis.y = hal_get_real(pins->axis[1]); + axis.z = hal_get_real(pins->axis[2]); + xdir.x = hal_get_real(pins->xdir[0]); + xdir.y = hal_get_real(pins->xdir[1]); + xdir.z = hal_get_real(pins->xdir[2]); + n = -1; + if (axis.x != 0 || axis.y != 0 || axis.z != 0) { + n = kinematicsToolFrameInverse(&axis, hal_get_bool(pins->have_x) ? &xdir : NULL, + j, hal_get_ui32(pins->held), sols, NSOL, + freed, spin); + } + hal_set_si32(pins->nsol, n); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + hal_set_real(pins->sol[k][i], k < n ? sols[k * joints + i] : 0.0); + } + hal_set_real(pins->spin[k], k < n ? spin[k] : 0.0); + hal_set_si32(pins->free[k], k < n ? freed[k] : 0); + } + + hal_set_ui32(pins->done, hal_get_ui32(pins->request)); +} + +int rtapi_app_main(void) +{ + static const char letter[3] = { 'x', 'y', 'z' }; + int i, k, r, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("twpcheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->j[i], 0.0, "twpcheck.j-%d", i); + } + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->axis[i], 0.0, "twpcheck.axis-%c", letter[i]); + res += hal_pin_new_real(comp_id, HAL_IN, &pins->xdir[i], 0.0, "twpcheck.xdir-%c", letter[i]); + } + res += hal_pin_new_bool(comp_id, HAL_IN, &pins->have_x, 0, "twpcheck.have-x"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->held, 0, "twpcheck.held"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->request, 0, "twpcheck.request"); + res += hal_pin_new_ui32(comp_id, HAL_OUT, &pins->done, 0, "twpcheck.done"); + for (r = 0; r < 3; r++) { + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->tool[r][i], 0.0, "twpcheck.tool-%d%d", r, i); + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->work[r][i], 0.0, "twpcheck.work-%d%d", r, i); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->frame_rc, 0, "twpcheck.frame-rc"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->nsol, 0, "twpcheck.nsol"); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->sol[k][i], 0.0, "twpcheck.sol-%d-%d", k, i); + } + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->spin[k], 0.0, "twpcheck.spin-%d", k); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->free[k], 0, "twpcheck.free-%d", k); + } + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + if (hal_export_funct("twpcheck", update, NULL, 1, 0, comp_id)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); }