diff --git a/configs/sim/axis/vismach/puma/mcodes/M128 b/configs/sim/axis/vismach/puma/mcodes/M128 index 26375100367..145f76e0015 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M128 +++ b/configs/sim/axis/vismach/puma/mcodes/M128 @@ -9,12 +9,11 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# RESTORE INI axis limits +# RESTORE INI axis velocity and acceleration limits (the position +# limits are never changed: a box on the machine frame, the flange) foreach l $coords { set L [string toupper $l] - catch {hal setp ini.$l.min_limit [set ::AXIS_[set L](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::AXIS_[set L](MAX_LIMIT)]} - catch {hal setp ini.$l.min_velocity [set ::AXIS_[set L](MAX_VELOCITY)]} + catch {hal setp ini.$l.max_velocity [set ::AXIS_[set L](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::AXIS_[set L](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/mcodes/M129 b/configs/sim/axis/vismach/puma/mcodes/M129 index f9f9a2d6b84..7fe3c4e7e19 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M129 +++ b/configs/sim/axis/vismach/puma/mcodes/M129 @@ -1,5 +1,5 @@ #!/usr/bin/tclsh -# switch kinematics and change axis limits to equiv joint limits +# switch kinematics and change axis velocity limits to equiv joint limits set kinstype 1 ;# 1: IDENTITY kins set switchkins_pin 3 ;# agree with inifile set max_joints 6 ;# agree with inifile @@ -10,11 +10,12 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# SET axis limits for IDENTITY kins using INI joint limits +# SET axis velocity and acceleration limits for IDENTITY kins using +# INI joint limits. The position limits stay: they are a box on the +# machine frame, the flange, which motion reads through the arm +# kinematics whatever type is in force for {set j 0} {$j < $max_joints} {incr j} { set l [lindex $coords $j] - catch {hal setp ini.$l.min_limit [set ::JOINT_[set j](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::JOINT_[set j](MAX_LIMIT)]} catch {hal setp ini.$l.max_velocity [set ::JOINT_[set j](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::JOINT_[set j](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/mcodes/M130 b/configs/sim/axis/vismach/puma/mcodes/M130 index 8051f2a2237..dcdede0fa04 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M130 +++ b/configs/sim/axis/vismach/puma/mcodes/M130 @@ -1,5 +1,5 @@ #!/usr/bin/tclsh -# switch kinematics and change axis limits +# switch kinematics and change axis velocity limits set kinstype 2 ;# 2: TYPE 2 kins set switchkins_pin 3 ;# agree with inifile set max_joints 6 ;# agree with inifile @@ -10,11 +10,12 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# SET axis limits for IDENTITY kins using INI joint limits +# SET axis velocity and acceleration limits for IDENTITY kins using +# INI joint limits. The position limits stay: they are a box on the +# machine frame, the flange, which motion reads through the arm +# kinematics whatever type is in force for {set j 0} {$j < $max_joints} {incr j} { set l [lindex $coords $j] - catch {hal setp ini.$l.min_limit [set ::JOINT_[set j](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::JOINT_[set j](MAX_LIMIT)]} catch {hal setp ini.$l.max_velocity [set ::JOINT_[set j](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::JOINT_[set j](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/puma.ini b/configs/sim/axis/vismach/puma/puma.ini index 99c0fa33112..f40fb5849c9 100644 --- a/configs/sim/axis/vismach/puma/puma.ini +++ b/configs/sim/axis/vismach/puma/puma.ini @@ -33,7 +33,7 @@ RS274NGC_STARTUP_CODE = G21 G10L2P0 x450 y100 z-495 a-180 (debug, ini: startup o MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma560.ini b/configs/sim/axis/vismach/puma/puma560.ini index 046f5d118d5..3d181caff47 100644 --- a/configs/sim/axis/vismach/puma/puma560.ini +++ b/configs/sim/axis/vismach/puma/puma560.ini @@ -36,7 +36,7 @@ PARAMETER_FILE = puma560.var MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma560_uvw.ini b/configs/sim/axis/vismach/puma/puma560_uvw.ini index a6b9f8544e9..b4d84d0e71c 100644 --- a/configs/sim/axis/vismach/puma/puma560_uvw.ini +++ b/configs/sim/axis/vismach/puma/puma560_uvw.ini @@ -35,7 +35,7 @@ PARAMETER_FILE = puma560.var MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma_cube.ini b/configs/sim/axis/vismach/puma/puma_cube.ini index 8397ce3ea7a..2742f6d1059 100644 --- a/configs/sim/axis/vismach/puma/puma_cube.ini +++ b/configs/sim/axis/vismach/puma/puma_cube.ini @@ -113,7 +113,7 @@ POSTGUI_HALFILE = puma_postgui.hal MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 37d63eaa701..e8aabf083d6 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1040,6 +1040,10 @@ The __ specifies one of: X Y Z A B C U V W The preview, the DROs, touch off and jogging of the GUIs follow the type too. * `MAX_VELOCITY = 1.2` - (real) Maximum velocity for this axis in <> per second. * `MAX_ACCELERATION = 20.0` - (real) Maximum acceleration for this axis in machine units per second squared. ++ +`MAX_VELOCITY` and `MAX_ACCELERATION` bound the letter in the world of the kinematics in force. +The joints are bound on their own: every move is capped at what its joints can follow within their `[JOINT_N]` limits, read through the kinematics module's Jacobian along the move, and a world jog the same way every servo cycle, so on a kinematics that is not the identity a move or a jog runs at the pace of its slowest joint whatever these say. +See the <> chapter, INI file limit settings. * `MAX_JERK = 0.0` - (real) Maximum jerk for this axis in machine units per second cubed. Used when S-curve trajectory planning is enabled. When set to 0 (default), no per-axis jerk limiting is applied. @@ -1051,6 +1055,11 @@ The __ specifies one of: X Y Z A B C U V W When this limit is exceeded, the controller aborts axis motion. The axis must be homed before MAX_LIMIT is in force. For a rotary axis (A,B,C typ) with unlimited rotation having no `MAX_LIMIT` for that axis in the `[AXIS_``]` section a value of 1e99 is used. ++ +`MIN_LIMIT` and `MAX_LIMIT` together are a box in the machine frame: the carriage, or the flange of a robot, in machine coordinates, the frame that does not move when a head tilts or a tool is loaded. +On a kinematics module that declares its machine frame type (see the <> chapter) every move is checked against the box where its joints put the machine frame, whatever kinematics type is in force: a tool centre point move whose tip lies past the box with the carriage inside it is accepted, and a 'G53.5' to a carriage position past the box is refused. +The travel itself is the `[JOINT_N]` limits; the box is a restriction the configuration adds inside it. +A module with one kinematics type whose joints are its slides, maxkins, reads the box on its joints, each in its letter. A module that declares no machine frame type and whose joints are not its slides keeps the box on the world of the kinematics in force. * `WRAPPED_ROTARY = 1` - (bool) When this is set to 1 for an ANGULAR axis the axis will move 0-359.999 degrees. Positive Numbers will move the axis in a positive direction and negative numbers will move the axis in the negative direction. It is ignored on a `LINEAR` axis. diff --git a/docs/src/gcode.html.in b/docs/src/gcode.html.in index 02353bbcf36..b573eafa3be 100644 --- a/docs/src/gcode.html.in +++ b/docs/src/gcode.html.in @@ -65,6 +65,11 @@ tr.head td, tr.head th { background: black; color: white; } G33K ($)Spindle Synchronized Motion G33.1K ($)Rigid Tapping G80 Cancel Canned Cycle + G53.4Point-to-Point Move, Program Coordinates + G53.5Point-to-Point Move, Machine Frame + G53.7J<n>=Point-to-Point Move, Joint Values + G53.1, G53.3, G53.6(P) (Q)Orient the Tool to the Work Plane + G53.2(P) (Q)Solve the Tool Orientation, No Motion Canned cycles(X Y Z or U V W apply to canned cycles, depending on active plane) G70Q (X) (Z) (D) (E) (P)Lathe finishing cycle @@ -100,6 +105,8 @@ tr.head td, tr.head th { background: black; color: white; } G43 H Tool Length Offset G43.1 Dynamic Tool Length Offset G43.2 H Apply additional Tool Length Offset + G43.4 (H) Tool Length Offset on Primary Kinematics + G43.5 (H) Tool Length Offset, Tool Axis as a Vector G49 Cancel Tool Length Compensation Stopping @@ -136,6 +143,10 @@ tr.head td, tr.head th { background: black; color: white; } M52P0 (off) or P1 (on)Adaptive Feed Control M53P0 (off) or P1 (on)Feed Stop Control G54-G59.3Select Coordinate System + G68.2, G68.4P Q I J K RTilted Work Plane + G68.3(R)Tilted Work Plane from the Tool + G69Cancel Tilted Work Plane + G12.1, G13.1PSelect Kinematics Flow-control Codes o subSubroutines, sub/endsub call @@ -167,6 +178,7 @@ tr.head td, tr.head th { background: black; color: white; } G28, G28.1Go/Set Predefined Position G28.2PHome from G-code G30, G30.1Go/Set Predefined Position + G28.5, G30.5Go to Predefined Machine Frame Position G53Move in Machine Coordinates G52, G92Coordinate System Offset G92.1, G92.2Reset G92 Offsets diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 20007307b18..7469bbc6c2a 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -74,7 +74,7 @@ as the 'L number', and so on for any other letter. |<> |Plane Select |<> |Set Units of Measure |<> |Go to Predefined Position -|<> |Go to Predefined Slide Position +|<> |Go to Predefined Machine Frame Position |<> |Home from G-code |<> |Go to Predefined Position |<> |Spindle Synchronized Motion @@ -1199,16 +1199,16 @@ G30 Z2.5 (rapid to Z2.5 then to the Z location specified in #5183) The 'absolute' position is the machine coordinate system of the kinematics in force, see <>; <> is the form that moves -the slides, and with `MACHINE_MOVES_IN_MACHINE_FRAME` set 'G30' and 'G30.1' are -'G30.5' and its store whenever the kinematics in force is not the machine frame -kinematics. +in the machine frame, and with `MACHINE_MOVES_IN_MACHINE_FRAME` set 'G30' and +'G30.1' are 'G30.5' and its store whenever the kinematics in force is not the +machine frame kinematics. It is an error if : * Cutter Compensation is turned on [[gcode:g28.5]] -== G28.5, G30.5 Go to Predefined Slide Position(((G28.5 Go to Predefined Slide Position))) +== G28.5, G30.5 Go to Predefined Machine Frame Position(((G28.5 Go to Predefined Machine Frame Position))) [source,ngc] ---- diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index ef36858dfe4..8f99f221407 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -611,7 +611,9 @@ 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. +them. `kinslimits` is built on it, and so is the cap canon puts on every +segment's velocity and acceleration from the joint limits through the Jacobian +(see the Switchable Kinematics chapter, INI file limit settings). A module that does not provide the form keeps working as it did. It just cannot be evaluated outside realtime, which the loader reports. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 137d8f6f5c1..f12c25b527b 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -300,12 +300,48 @@ MAX_VELOCITY = MIN_ACCELERATION = ---- -The INI file limits specified apply to the type 0 default -kinematics type that is activated at startup. These limits may -*not* be applicable when switching to alternative kinematics. -However, since an interpreter-motion synchronization is required -when switching kinematics, INI-HAL pins can be used to setup -limits for a pending kinematics type. +The position limits, `MIN_LIMIT` and `MAX_LIMIT`, are a box in the +machine frame, the world of the kinstype the module declares +KINSTYPE_MACHINE (its identity kinstype where it declares none): +the carriage in machine coordinates, or a robot's flange in its base +frame. Every move is checked against that box where its joints put +the machine frame, whichever kinstype is in force, so the box means +the same thing after a switch as before it and needs no resetting. +The travel is the `[JOINT_N]` limits, checked on every move as well; +the box is a restriction inside it. A module that declares no +kinstype flags keeps the box on the world of the kinstype in force, +as before. + +The velocity and acceleration limits, `MAX_VELOCITY` and +`MAX_ACCELERATION`, bound the coordinate letter in the world of the +kinstype in force. What the joints can follow is bound separately: +every segment is sampled through the module's Jacobian ahead of +motion, on the kinstype and the tool offset the program is under, and +its velocity and acceleration are lowered to what the slowest joint +can give within its own `[JOINT_N]` limits, the share a carriage on a +circle feels as centripetal included. A turn of a tilted head under +the tool centre point, which swings the carriage through a circle the +head's own limit never mentions, runs at the carriage's pace. A +module that can only be evaluated in realtime (one without the +parameter block form, see <>) +caps nothing; on it the `[AXIS_L]` limits are all there is. A world +jog is bounded the same way, in motion, every servo cycle: the Jacobian +at the joints the machine stands in caps the jog's planners so that no +joint is asked for more than its limits, and a jog of C under the tool +centre point with the head laid over runs at the pace of the carriage +it swings. The cap reads the joints' travel too: where the carriage +would run onto its `[JOINT_N]` soft limit the jog slows and comes to rest +against it, as a jog of a slide does at its axis limit, with no error, +and a jog the other way moves off it. + +So a switch does not need the velocity and acceleration limits reset +to keep the joints within their limits. It may still want them reset +where a letter names a joint of another kind after the switch: a +robot's identity kinstype puts joint 0, a rotary, on X, and the +`[AXIS_X]` limit in mm/s says nothing useful about it. Since an +interpreter-motion synchronization is required when switching +kinematics, INI-HAL pins can be used to set up limits for a pending +kinematics type. [NOTE] INI-HAL pins are typically not recognized during a G-code program @@ -340,7 +376,7 @@ kinematics (trivkins). See the kins man page for more information ($ man kins). A user-provided M-code can alter any or all of the axis coordinate -limits prior to changing the motion.switchkins-type pin and +velocity and acceleration limits prior to changing the motion.switchkins-type pin and synchronizing the interpreter and motionparts of LinuxCNC. As an example, a bash script invoking halcmd can be 'hardcoded' to set any number of HAL pins: @@ -366,24 +402,25 @@ with a complex (non-identity) kinematics (type0) after homing. The system is configured so that it can be switched to identity kinematics (type1) in order to manipulate individual joints using the conventional letters from the set 'XYZABCUVW'. The INI file -settings ([AXIS_L]) are *not* applicable when operating with -identity (type1) kinematics. To address this use case, the user -M-code scripts can be designed as follows: +velocity and acceleration settings ([AXIS_L]) are *not* applicable +when operating with identity (type1) kinematics. To address this +use case, the user M-code scripts can be designed as follows: *M129* (Switch to identity type1) . read and parse INI file -. HAL: setp the INI-HAL limit pins for each axis letter ([AXIS_L]) - according to the 'identity-referenced' joint number INI file - setting ([JOINT_N]) +. HAL: setp the INI-HAL velocity and acceleration pins for each axis + letter ([AXIS_L]) according to the 'identity-referenced' joint + number INI file setting ([JOINT_N]) . HAL: `setp motion.switchkins-type 1` . MDI: execute a syncing G-code (M66E0L0) *M128* (restore robot default kinematics type 0) . read and parse INI file -. HAL: setp the INI-HAL limit pins for each axis letter ([AXIS_L]) - according to the appropriate INI file setting ([AXIS_L]) +. HAL: setp the INI-HAL velocity and acceleration pins for each axis + letter ([AXIS_L]) according to the appropriate INI file setting + ([AXIS_L]) . HAL: `setp motion.switchkins-type 0` . MDI: execute a syncing G-code (M66E0L0) diff --git a/docs/src/remap/remap.adoc b/docs/src/remap/remap.adoc index 75e2a0980fa..02c225dda19 100644 --- a/docs/src/remap/remap.adoc +++ b/docs/src/remap/remap.adoc @@ -1829,8 +1829,8 @@ All the listed G-codes are already defined in the current implementation of Linu |# |Gxx |Gxx.1 |Gxx.2 |Gxx.3 |Gxx.4 |Gxx.5 |Gxx.6 |Gxx.7 |Gxx.8 |Gxx.9 |10 |G10 | | | | | | | | | |11 | | | | | | | | | | -|12 | | | | | | | | | | -|13 | | | | | | | | | | +|12 | |G12.1 | | | | | | | | +|13 | |G13.1 | | | | | | | | |14 | | | | | | | | | | |15 | | | | | | | | | | |16 | | | | | | | | | | @@ -1851,7 +1851,7 @@ All the listed G-codes are already defined in the current implementation of Linu |25 | | | | | | | | | | |26 | | | | | | | | | | |27 | | | | | | | | | | -|28 |G28 |G28.1 |G28.2 | | | | | | | +|28 |G28 |G28.1 |G28.2 | | |G28.5 | | | | |29 | | | | | | | | | | |=== @@ -1859,10 +1859,10 @@ All the listed G-codes are already defined in the current implementation of Linu [width="90%",align="center",options="header,strong,unbreakable",cols="1*2^em,10*1. +""" + +G13_1 = """G13.1 Select Machine Frame Kinematics +Cancels back to the kinematics the module declares +the machine frame: the identity on a machine whose +slides line up with its frame, the arm kinematics +with no tool on a robot. Which number that is, the +module declares, so G13.1 means the same whatever +order the module lists its types in. A queue +synchronisation point like G12.1. +""" + G17 = """G17 Plane Select G17 = XY Plane """ @@ -492,6 +564,28 @@ def gcode_descriptions(gcode): or if Pn names a joint the machine does not have. """ +G28_5 = """G28.5 Go to Predefined Machine Frame Position +G28.5 + +G28.5 is to G28 what G53.5 is to G53: the position +stored in #5161-#5169 is read in the machine frame +with the orientation left out, the carriage or the +flange in machine coordinates, and the move is a +point-to-point move there, rapid, with no offset, +tool length, rotation or work plane applied. With +axes, those letters go to the given machine frame +position first, then to their stored positions. +Store the position with G28.1 while the machine +frame kinematics is in force (G13.1). +""" + +G30_5 = """G30.5 Go to Predefined Machine Frame Position +G30.5 + +As G28.5, with the position stored in #5181-#5189 +by G30.1. +""" + G30 = """G30 Go to Predefined Position G30 uses the values stored in parameters 5181-5189 as the X Y Z A B C U V W final point to move to. @@ -698,6 +792,37 @@ def gcode_descriptions(gcode): location. """ +G43_4 = """G43.4 Tool Length Offset on Primary Kinematics +G43.4 Hn +n = tool number, Hn is optional + +G43 together with a switch to the kinematics the +module declares its working transform, tool centre +point control, so the program runs with tool length +compensation in that kinematics. The switch happens +first, the offset applies after it. G49 cancels the +offset and switches back to the machine frame +kinematics. Rejected on a switchable module that +declares no primary kinematics; a plain G43 on a +machine that does not switch. +""" + +G43_5 = """G43.5 Tool Length Offset with the Tool Axis as a Vector +G43.5 Hn +G0 X- Y- Z- I- J- K- +G1 X- Y- Z- I- J- K- F- + +G43.4 with one thing more: while it is in effect a +G0 or G1 line may give the tool axis direction as a +vector I J K, from the tip towards the holder, in +place of rotary words, and the interpreter works +out where the rotaries have to go. The pose nearest +where the rotaries stand is taken. G2 and G3 keep +I J K as the arc centre. It is an error to give +I J K with a rotary word, or a zero vector, or on +the identity kinematics. +""" + G49 = """G49 Cancel Tool Length Compensation """ @@ -719,6 +844,101 @@ def gcode_descriptions(gcode): line if one is currently active. """ +G53_1 = """G53.1 Orient the Tool to the Work Plane +G53.1 + +Turns the rotaries until the tool axis is normal to +the active tilted work plane (G68.2). The linear +joints stay where they are and the tip swings with +them: a point-to-point move. P picks among the +poses that reach the plane, nearest first: P0 (the +default) the nearest, P1 and P2 the secondary +rotary positive or negative. Q0 (the default) holds +the joints that carry the work, Q1 frees them. An +error with no plane active, or on the identity +kinematics. +""" + +G53_2 = """G53.2 Solve the Tool Orientation without Moving +G53.2 + +Solves the same pose as G53.1 and moves nothing. +The pose is published on #<_orient_x> to +#<_orient_c> and on #5071 to #5079, in program +units in the plane, with #<_orient_valid> (#5080) +set to 1, so the program can reach it with a move +of its own, for instance a single G0 naming X Y Z +and the rotary words together. P and Q as for +G53.1. +""" + +G53_3 = """G53.3 Orient the Tool and Move in the Work Plane +G53.3 X- Y- Z- + +Turns the rotaries as G53.1 does and takes the tool +to X Y Z, given in the plane, in one point-to-point +move. A word left out keeps the present value. P +and Q as for G53.1. +""" + +G53_4 = """G53.4 Point-to-Point Move in Program Coordinates +G53.4 G0 +G53.4 G1 F- + +A point-to-point move defines its two ends and +leaves the path to the joints: the inverse +kinematics runs once at the destination and every +joint travels from where it is to where it must be, +all together, the slowest setting the pace. The +tool does not follow a straight line. G53.4 takes +the destination in program coordinates, through +the offsets and the work plane like any other +move: the move to cross a singularity or turn a +head right round without leaving the coordinate +system. Non-modal, on a line with G0 or G1 in +force; with G1 it takes the time the straight move +would at F. +""" + +G53_5 = """G53.5 Point-to-Point Move in the Machine Frame +G53.5 G0 +G53.5 G1 F- + +A point-to-point move (see G53.4) to a position in +the machine frame with the tool left out: X Y Z the +point the tool hangs from, the pivot of a head or +the flange of a robot, in machine coordinates, the +rotary letters the orientation as the machine frame +kinematics reports it, on a mill the rotary joints. +Program units, no offset, tool length, rotation or +work plane. A letter not given holds its machine +frame coordinate. The form for parking and tool +change positions, whatever the head is doing. +Absolute distance mode only. +""" + +G53_6 = """G53.6 Orient the Tool, Holding the Tool Centre Point +G53.6 + +Turns the rotaries until the tool is normal to the +active work plane, as G53.1, keeping the tool +centre point where it is: a Cartesian move of the +rotary words, the kinematics compensating the +linear joints all along. P and Q as for G53.1. +""" + +G53_7 = """G53.7 Point-to-Point Move to Joint Values +G53.7 G0 J= ... +G53.7 G1 F- J= ... + +A point-to-point move (see G53.4) to joint values, +one word per joint: J2=-5 sends joint 2 to -5 in +the joint's own units, nothing converted, no offset +of any kind. A joint not named holds. The form that +works on every machine, a robot included. Absolute +distance mode only; the J= word is read only here. +""" + G54 = """G54 Select Coordinate System G54 = select coordinate system 1 """ @@ -788,6 +1008,51 @@ def gcode_descriptions(gcode): linear move. """ +G68_2 = """G68.2 Tilted Work Plane +G68.2 X- Y- Z- I- J- K- (Euler angles) +G68.2 P1 X- Y- Z- I- J- K- (angles about fixed axes) +G68.2 P2 Q0..Q3 X- Y- Z- (three points, one per block) +G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: origin and +X,) +G68.2 P3 Q2 I- J- K- (then +Z, the normal) + +Defines a plane on top of the active coordinate +system, with its own cancel (G69), so the blocks +that follow are programmed in the tilted plane +while G54 itself is untouched. X Y Z is the plane's +origin, the rest of the words its rotation, both in +the coordinate system active when the plane is +defined. P selects the form, numbered as Fanuc +numbers them; Q names the axis order of the angle +forms (Q313 and Q123 the defaults) or the block of +the point and vector forms. R turns the plane about +its own Z after everything else. The plane does not +move the tool: G53.1, G53.3 or G53.6 orient it. +""" + +G68_3 = """G68.3 Tilted Work Plane from the Tool Direction +G68.3 X- Y- Z- + +Defines a plane whose Z is the tool axis as the +rotary joints stand now, with the origin at X Y Z +and R turning it about that Z. An error where the +kinematics cannot be evaluated by the interpreter, +is the identity, or reports no tool frame. +""" + +G68_4 = """G68.4 Tilted Work Plane on the Active Plane +G68.4 (any G68.2 form) + +Takes any G68.2 form and composes it onto the plane +already active, so the new plane is given in the +coordinates of the old one. An error with no plane +active. +""" + +G69 = """G69 Cancel Tilted Work Plane +Cancels the plane. So does the end of the program, +M2 or M30, and an abort. +""" + G73 = """G73 Drilling Cycle with Chip Breaking G73 X Y Z R Q L R = retract position along the Z axis diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 839313ef965..fb08407b951 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -150,6 +150,17 @@ int kinematicsTypeFlags(int ktype) return -1; } +// and so no machine frame type either: the carriage is the joints, where +// they are the slides, and otherwise there is no machine frame to read +int kinematicsMachineFrame(const double *joint, EmcPose *pos) +{ + if (!inited || !joint || !pos) { return -1; } + if (ops()->slides) { + return kinsMappedJointsToPose(&rt_params, joint, pos) == 0 ? 0 : -2; + } + return -1; +} + // 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. @@ -171,5 +182,6 @@ EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinematicsMachineFrame); EXPORT_SYMBOL(kinsSingleInit); EXPORT_SYMBOL(kinsSingleDescribe); diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index 186873a014a..cb0cd653d13 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -214,6 +214,7 @@ static const kins_ops max_ops = { .tool = max_tool_frame, .native = &TOOL_FRAME_SPINDLE, .jacobian = max_jacobian, + .slides = 1, }; // joints 0..8 are X..W in order, always; the entry points come from diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index ac16fc36c4e..e2f92709ebd 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -475,6 +475,20 @@ int kinematicsTypeFlags(int ktype) return ktype_flags[ktype]; } // kinematicsTypeFlags() +int kinematicsMachineFrame(const double *joint, EmcPose *pos) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + int k; + + for (k = 0; k < kins_count; k++) { + int f = kinematicsTypeFlags(k); + if (f < 0 || !(f & KINSTYPE_MACHINE)) { continue; } + return call_forward(k, joint, pos, &fflags, &iflags) == 0 ? 0 : -2; + } + return -1; +} // kinematicsMachineFrame() + int switchkinsRegisterOps(int ktype, const kins_ops *ops) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -583,6 +597,7 @@ EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinematicsMachineFrame); EXPORT_SYMBOL(switchkinsRegisterJacobian); EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 24e37b47d52..a734fbcc467 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -532,9 +532,7 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, { 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; @@ -546,6 +544,24 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, world, j, &iflags, &fflags) != 0) { return -1; } + return kinematicsUserJacobianAt(ctx, j, world, J); +} + +int kinematicsUserJacobianAt(KinematicsUserContext* ctx, + const double* joints, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double j[EMCMOT_MAX_JOINTS] = {0}; + int r, a; + + if (!ctx || !ctx->initialized || !joints || !world || !J) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + for (r = 0; r < KINEMATICS_USER_MAX_JOINTS; r++) j[r] = joints[r]; if (kinsOpsJacobian(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, world, jac, &iflags) != 0) { return -1; diff --git a/src/emc/motion/axis.c b/src/emc/motion/axis.c index 4aa4a26ba13..6bbaf08ad16 100644 --- a/src/emc/motion/axis.c +++ b/src/emc/motion/axis.c @@ -26,6 +26,8 @@ typedef struct { double acc_limit; /* upper limit of axis accel */ double jerk_limit; /* upper limit of axis jerk */ simple_tp_t teleop_tp; /* planner for teleop mode motion */ + double teleop_vel_req; /* the speed the jog asked for */ + double teleop_acc_req; /* and the acceleration it may use */ rtapi_sint old_ajog_counts; /* prior value, used for deltas */ int kb_ajog_active; /* non-zero during a keyboard jog */ @@ -336,6 +338,8 @@ void axis_jog_cont(int axis_num, double vel, long servo_period) axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -359,6 +363,8 @@ void axis_jog_incr(int axis_num, double offset, double vel, long servo_period) axis->teleop_tp.pos_cmd = tmp1; axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -381,6 +387,8 @@ void axis_jog_abs(int axis_num, double offset, double vel) axis->teleop_tp.pos_cmd = tmp1; axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -490,6 +498,8 @@ void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, boo axis->teleop_tp.pos_cmd = pos; axis->teleop_tp.max_vel = axis->vel_limit; axis->teleop_tp.max_acc = aaccel_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->wheel_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -746,6 +756,32 @@ static int update_teleop_with_check(int axis_num, simple_tp_t *the_tp, double se return 0; } +// Whether the axis's teleop planner has somewhere to go this cycle, and +// which way, how fast and how hard the jog asked it to: what the cap on +// the joints reads before the planners run +int axis_teleop_request(int axis_num, double *dir, double *vel, double *acc) +{ + emcmot_axis_t *axis = &axis_array[axis_num]; + double togo = axis->teleop_tp.pos_cmd - axis->teleop_tp.curr_pos; + + if (!axis->teleop_tp.enable) { return 0; } + if (fabs(togo) < TINY_DP(axis->teleop_tp.max_acc, 0.001) && axis->teleop_tp.curr_vel == 0.0) { return 0; } + *dir = togo > 0.0 ? 1.0 : togo < 0.0 ? -1.0 : axis->teleop_tp.curr_vel > 0.0 ? 1.0 : -1.0; + *vel = axis->teleop_vel_req; + *acc = axis->teleop_acc_req; + return 1; +} + +// The most the planner may do this cycle: what the jog asked, or less +// where the joints cannot follow that +void axis_teleop_cap(int axis_num, double vel, double acc) +{ + emcmot_axis_t *axis = &axis_array[axis_num]; + + axis->teleop_tp.max_vel = vel < axis->teleop_vel_req ? vel : axis->teleop_vel_req; + axis->teleop_tp.max_acc = acc < axis->teleop_acc_req ? acc : axis->teleop_acc_req; +} + int axis_calc_motion(double servo_period) { int axis_num; diff --git a/src/emc/motion/axis.h b/src/emc/motion/axis.h index 5d61f0b13f0..bc00504570d 100644 --- a/src/emc/motion/axis.h +++ b/src/emc/motion/axis.h @@ -51,6 +51,8 @@ void axis_apply_ext_offsets_to_carte_pos(int extfactor, double *pcmd_p[]); int axis_update_coord_with_bound(double *pcmd_p[], double servo_period); +int axis_teleop_request(int axis_num, double *dir, double *vel, double *acc); +void axis_teleop_cap(int axis_num, double vel, double acc); int axis_calc_motion(double servo_period); diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index b669000e664..33d9a149416 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -74,6 +74,8 @@ // 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 +// old modules export no kinematicsMachineFrame; keep it optional +#pragma weak kinematicsMachineFrame #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -270,47 +272,66 @@ void apply_spindle_limits(spindle_status_t *s){ } -/* inRange() returns non-zero if the position lies within the joint - limits, or 0 if not. It also reports an error for each joint limit - violation. It's possible to get more than one violation per move. */ -STATIC int inRange(EmcPose pos, int id, char *move_type) +/* The [AXIS_L] box is the machine frame envelope: the carriage, or the + flange, in machine coordinates, the frame that does not move when the + head tilts or a tool is loaded. Where the module names its machine + frame type, the endpoint's joints go through that type's forward and + the box is read there, whatever type is in force; a module that names + none keeps the box on the world of the type in force, as before. + Returns non-zero when the endpoint is inside the box, and reports each + letter outside it. */ +STATIC int box_ok(const double *joint_pos, const EmcPose *pos, int id, const char *move_type) { - double joint_pos[EMCMOT_MAX_JOINTS]; - int joint_num, axis_num; - emcmot_joint_t *joint; - int in_range = 1; + EmcPose frame = *pos; /* the estimate in, and the fallback */ int failing_axes[EMCMOT_MAX_AXIS]; double targets[EMCMOT_MAX_AXIS]; const char axis_letters[] = "XYZABCUVW"; + int axis_num, in_box = 1; if (EMCMOT_MAX_AXIS != 9) { rtapi_print_msg(RTAPI_MSG_ERR, "BUG: %s(): invalid number of axes defined", __func__); - } else { - targets[0] = pos.tran.x; - targets[1] = pos.tran.y; - targets[2] = pos.tran.z; - targets[3] = pos.a; - targets[4] = pos.b; - targets[5] = pos.c; - targets[6] = pos.u; - targets[7] = pos.v; - targets[8] = pos.w; - axis_check_constraints(targets, failing_axes); - for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num += 1) { - if (failing_axes[axis_num] == -1) { - reportError(_("%s move on line %d would exceed %c's %s limit"), - move_type, id, axis_letters[axis_num], _("negative")); - in_range = 0; - } - if (failing_axes[axis_num] == 1) { - reportError(_("%s move on line %d would exceed %c's %s limit"), - move_type, id, axis_letters[axis_num], _("positive")); - in_range = 0; - } + return 1; + } + if (kinematicsMachineFrame && kinematicsMachineFrame(joint_pos, &frame) == -2) { + reportError(_("%s move on line %d cannot be placed in the machine frame"), + move_type, id); + return 0; + } + targets[0] = frame.tran.x; + targets[1] = frame.tran.y; + targets[2] = frame.tran.z; + targets[3] = frame.a; + targets[4] = frame.b; + targets[5] = frame.c; + targets[6] = frame.u; + targets[7] = frame.v; + targets[8] = frame.w; + axis_check_constraints(targets, failing_axes); + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num += 1) { + if (failing_axes[axis_num] == -1) { + reportError(_("%s move on line %d would exceed %c's %s limit"), + move_type, id, axis_letters[axis_num], _("negative")); + in_box = 0; + } + if (failing_axes[axis_num] == 1) { + reportError(_("%s move on line %d would exceed %c's %s limit"), + move_type, id, axis_letters[axis_num], _("positive")); + in_box = 0; } } + return in_box; +} - /* Now, check that the endpoint puts the joints within their limits too */ +/* inRange() returns non-zero if the position lies within the joint + limits and the [AXIS_L] box, or 0 if not. It also reports an error for + each limit violation. It's possible to get more than one violation per + move. */ +STATIC int inRange(EmcPose pos, int id, char *move_type) +{ + double joint_pos[EMCMOT_MAX_JOINTS]; + int joint_num; + emcmot_joint_t *joint; + int in_range = 1; /* start the inverse from where the queue leaves the joints */ queue_end_joints(joint_pos); @@ -324,6 +345,10 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) return 0; } + /* the box, read where the joints put the machine frame */ + if (!box_ok(joint_pos, &pos, id, move_type)) { in_range = 0; } + + /* and the joints within their limits */ for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { /* point to joint data */ joint = &joints[joint_num]; @@ -1239,6 +1264,14 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) SET_MOTION_ERROR_FLAG(1); break; } + /* the joints named still have to keep the machine frame in the box */ + if (!box_ok(target, &end, emcmotCommand->id, "Joint interpolated")) { + reportError(_("invalid params in joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } } else { if (!inRange(end, emcmotCommand->id, "Joint interpolated")) { reportError(_("invalid params in joint interpolated move")); diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 22cbfdaf5a4..b7403cd8345 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -46,6 +46,10 @@ static int switchkins_type = 0; KINEMATICS_FORWARD_FLAGS fflags = 0; KINEMATICS_INVERSE_FLAGS iflags = 0; +// old modules export no kinematicsJacobian; a jog on them is bounded by +// the axis limits alone, as before +#pragma weak kinematicsJacobian + /*! \todo FIXME - debugging - uncomment the following line to log changes in JOINT_FLAG and MOTION_FLAG */ // #define WATCH_FLAGS 1 @@ -1344,6 +1348,149 @@ static void handle_jjogwheels(void) first_pass = 0; } +/* A world jog is planned per axis against the [AXIS_L] limits, which say + nothing about the joints once the kinematics is not the identity: a jog + of C under the tool centre point with the head laid over swings the + carriage through a circle. Each cycle, before the teleop planners run, + the module's Jacobian at the joints the machine stands in says how fast + every joint would move per unit of the jog the planners were asked for, + and how that rate has changed since the last cycle (what a carriage on a + circle feels as centripetal), and the planners are capped so that no + joint is asked for more than its own limits: the velocity by the joint + velocity limits, read a stopping distance ahead since the rate keeps + growing while the planners slow down, and by the acceleration budget + the changing rate takes at that velocity (at most half); the + acceleration by what the budget leaves. All active axes scale together + so the jog keeps its direction. Nothing is done on the identity, or on + a module without a Jacobian. */ +static void teleop_joint_cap(double period) +{ + static double jac_prev[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + static int have_prev = 0; + static double acc_scale_prev = 1.0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double joint_pos[EMCMOT_MAX_JOINTS]; + double dir[EMCMOT_MAX_AXIS], vreq[EMCMOT_MAX_AXIS], areq[EMCMOT_MAX_AXIS]; + double rhat[EMCMOT_MAX_AXIS], ahat[EMCMOT_MAX_AXIS]; + int active[EMCMOT_MAX_AXIS]; + double rnorm = 0.0, anorm = 0.0, speed = 0.0, ahead, vcap = 1e99, acap = 1e99; + double vel_scale, acc_scale, stop_acc; + const double tiny = 1e-12; + int flags, any = 0, a, j, pinned = 0; + + if (!kinematicsJacobian) { have_prev = 0; return; } + flags = emcmotStatus->switchkins_flags[emcmotStatus->switchkins_type]; + if (flags >= 0 ? (flags & KINSTYPE_IDENTITY) != 0 + : emcmotConfig->kinType == KINEMATICS_IDENTITY) { + have_prev = 0; + return; + } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + active[a] = axis_teleop_request(a, &dir[a], &vreq[a], &areq[a]); + if (active[a]) { + double v = axis_get_teleop_vel_cmd(a); + any = 1; + rnorm += vreq[a] * vreq[a]; + anorm += areq[a] * areq[a]; + speed += v * v; + } + } + if (!any) { have_prev = 0; return; } + rnorm = sqrt(rnorm); + anorm = sqrt(anorm); + speed = sqrt(speed); + if (rnorm < tiny || anorm < tiny) { have_prev = 0; return; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } + if (kinematicsJacobian(joint_pos, &emcmotStatus->carte_pos_cmd, jac, &iflags) != 0) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (active[a]) { axis_teleop_cap(a, vreq[a], areq[a]); } + } + have_prev = 0; + return; + } + /* the direction the jog was asked in, and the one the planners + accelerate in, each at its own limit */ + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + rhat[a] = active[a] ? dir[a] * vreq[a] / rnorm : 0.0; + ahat[a] = active[a] ? dir[a] * areq[a] / anorm : 0.0; + } + /* the path the planners need to stop from the speed they are at, + at the acceleration they were left last cycle */ + ahead = speed * speed / (2.0 * anorm * acc_scale_prev); + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double rate = 0.0, change = 0.0, cap; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + rate += jac[j][a] * rhat[a]; + if (have_prev) { change += (jac[j][a] - jac_prev[j][a]) * rhat[a]; } + } + rate = fabs(rate); + /* how the rate changes per unit of path, from the last cycle */ + change = (have_prev && speed > tiny) ? fabs(change) / (speed * period) : 0.0; + if (rate > tiny) { + cap = joints[j].vel_limit / (rate + change * ahead); + if (cap < vcap) { vcap = cap; } + } + if (change > tiny) { + cap = sqrt(joints[j].acc_limit / (2.0 * change)); + if (cap < vcap) { vcap = cap; } + } + joint_pos[j] = change; /* kept for the acceleration pass */ + } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double arate = 0.0, cap; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { arate += jac[j][a] * ahat[a]; } + arate = fabs(arate); + if (arate > tiny) { + cap = (joints[j].acc_limit - joint_pos[j] * vcap * vcap) / arate; + if (cap < acap) { acap = cap; } + } + } + /* the soft limits: a joint heading for one must still be able to stop + at it, at half the acceleration the planners are left, and gets no + further than it in one cycle. A jog that has come to rest against + one ends there, as a jog of a slide ends at its axis limit, and a + jog away from it is free. The room is read from where the planners + have taken the joint, not from the interpolated command that follows + it a few cycles behind, and a ten-millionth of the joint's range + short, for the curve of the path over the last step; within one + more the jog is at rest there */ + stop_acc = 0.5 * (acap < anorm ? acap : anorm); + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double rate = 0.0, room, margin, cap; + if (!GET_JOINT_ACTIVE_FLAG(&joints[j]) || !get_homed(j)) { continue; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { rate += jac[j][a] * rhat[a]; } + if (fabs(rate) < tiny) { continue; } + room = rate > 0.0 ? joints[j].max_pos_limit - joints[j].coarse_pos + : joints[j].coarse_pos - joints[j].min_pos_limit; + margin = 1e-7 * (joints[j].max_pos_limit - joints[j].min_pos_limit); + room -= margin; + if (room <= margin) { + cap = 0.0; + } else { + cap = sqrt(2.0 * stop_acc * room / fabs(rate)); + if (room / (fabs(rate) * period) < cap) { cap = room / (fabs(rate) * period); } + } + if (cap < vcap) { + vcap = cap; + pinned = cap < tiny; + } + } + vel_scale = vcap / rnorm; + acc_scale = acap / anorm; + if (vel_scale > 1.0) { vel_scale = 1.0; } + if (acc_scale > 1.0) { acc_scale = 1.0; } + if (acc_scale < 1e-6) { acc_scale = 1e-6; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (active[a]) { axis_teleop_cap(a, vel_scale * vreq[a], acc_scale * areq[a]); } + } + if (pinned && speed < tiny) { axis_jog_abort_all(0); } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac_prev[j][a] = jac[j][a]; } + } + have_prev = 1; + acc_scale_prev = acc_scale; +} // teleop_joint_cap() + static void get_pos_cmds(long period) { int joint_num, result; @@ -1655,6 +1802,7 @@ static void get_pos_cmds(long period) break; case EMCMOT_MOTION_TELEOP: + teleop_joint_cap(servo_period); ext_offset_teleop_limit = axis_calc_motion(servo_period); if (!ext_offset_teleop_limit) { ext_offset_coord_limit = 0; //in case was set in prior coord motion diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile index 54a9b384935..5ec4c036a9f 100644 --- a/src/emc/motion_planning/Submakefile +++ b/src/emc/motion_planning/Submakefile @@ -6,6 +6,7 @@ INCLUDES += emc/kinematics_userspace LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ jacobian.cc \ joint_limits.cc \ + segment_cap.cc \ ) # kins_util.c is the shared kinematics code the modules link; the loader @@ -45,7 +46,8 @@ USERSRCS += $(KINSLIMITS_SRCS) TARGETS += ../bin/kinslimits -MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh \ + emc/motion_planning/segment_cap.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/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc index 1b750be6776..04ca010defc 100644 --- a/src/emc/motion_planning/kinslimits.cc +++ b/src/emc/motion_planning/kinslimits.cc @@ -6,8 +6,8 @@ * * 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. + * the most restrictive cap found along it, then the cap canon puts on + * the segment through segmentCap(), which is the one the planner sees. * * Example (in a terminal with a running config, or under halrun): * @@ -36,6 +36,7 @@ #include #include "jacobian.hh" #include "joint_limits.hh" +#include "segment_cap.hh" using namespace motion_planning; @@ -263,6 +264,32 @@ int main(int argc, char **argv) min_vel, at_vel, min_vel_s, min_acc, at_acc, min_jerk, at_jerk); printf("worst cond : %.3f\n", max_cond); + /* the cap canon reads for the same move, along the same tangent, with + the change of the joint rates along the move charged as well */ + { + struct { EmcPose start, end; } line = { start, end }; + SegmentCapLimits lim; + SegmentCap cap; + double joints[KINEMATICS_USER_MAX_JOINTS] = {0}; + lim.joints = num_joints; + for (int j = 0; j < KINEMATICS_USER_MAX_JOINTS; j++) { + lim.vel[j] = j < num_joints ? vel_v[j] : SEGMENT_CAP_NONE; + lim.acc[j] = j < num_joints ? acc_v[j] : SEGMENT_CAP_NONE; + } + auto pose_at = [](double f, EmcPose *p, void *arg) { + const EmcPose *se = (const EmcPose *)arg; + for (int ax = 0; ax < 9; ax++) { + set_pose_axis(p, ax, pose_axis(se[0], ax) + f * (pose_axis(se[1], ax) - pose_axis(se[0], ax))); + } + }; + if (segmentCap(ctx, &lim, target, samples, pose_at, &line, joints, &cap) == 0) { + printf("canon's cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), %d of %d samples unanswered\n", + cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered, cap.samples); + } else { + printf("canon's cap : none, no sample could be evaluated\n"); + } + } + kinematicsUserFree(ctx); hal_exit(comp_id); return 0; diff --git a/src/emc/motion_planning/segment_cap.cc b/src/emc/motion_planning/segment_cap.cc new file mode 100644 index 00000000000..ec4cf00faf0 --- /dev/null +++ b/src/emc/motion_planning/segment_cap.cc @@ -0,0 +1,175 @@ +/******************************************************************** + * Description: segment_cap.cc + * The velocity and acceleration cap a segment gets from the joint + * limits through the kinematics module's Jacobian. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + ********************************************************************/ + +#include "segment_cap.hh" +#include + +namespace motion_planning { + +static double pose_axis(const EmcPose &p, int ax) +{ + switch (ax) { + case AXIS_X: return p.tran.x; + case AXIS_Y: return p.tran.y; + case AXIS_Z: return p.tran.z; + case AXIS_A: return p.a; + case AXIS_B: return p.b; + case AXIS_C: return p.c; + case AXIS_U: return p.u; + case AXIS_V: return p.v; + default: return p.w; + } +} + +int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int samples, SegmentPoseFn pose_at, void *arg, + double *joints, SegmentCap *out) +{ + const double tiny = 1e-12; + /* the rate of every joint per unit of path parameter at each sample, + the most that rate changes per unit of parameter on either side of + it, and the most it can bulge past the samples on either side */ + static double g[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double h[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double bulge[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double s[SEGMENT_CAP_MAX_SAMPLES]; + static bool ok[SEGMENT_CAP_MAX_SAMPLES]; + int K = samples < 2 ? 2 : samples; + int njoints; + int k, j, a, prev, answered = 0; + + out->vel = SEGMENT_CAP_NONE; + out->acc = SEGMENT_CAP_NONE; + out->vel_joint = -1; + out->acc_joint = -1; + out->vel_at = 0.0; + out->samples = 0; + out->unanswered = 0; + if (!ctx || !lim || !pose_at || !joints || length <= 0.0) { return -1; } + if (K > SEGMENT_CAP_MAX_SAMPLES) { K = SEGMENT_CAP_MAX_SAMPLES; } + njoints = lim->joints; + if (njoints > KINEMATICS_USER_MAX_JOINTS) { njoints = KINEMATICS_USER_MAX_JOINTS; } + out->samples = K; + + for (k = 0; k < K; k++) { + double f = (double)k / (double)(K - 1); + /* the tangent by a central difference of the geometry, exact for + a line and a small fraction of a sample step for an arc */ + double df = 1.0 / (8.0 * (K - 1)); + double fp = f + df > 1.0 ? 1.0 : f + df; + double fm = f - df < 0.0 ? 0.0 : f - df; + EmcPose p, pp, pm; + double t[AXIS_COUNT]; + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]; + + s[k] = f * length; + ok[k] = false; + pose_at(f, &p, arg); + pose_at(fp, &pp, arg); + pose_at(fm, &pm, arg); + for (a = 0; a < AXIS_COUNT; a++) { + t[a] = (pose_axis(pp, a) - pose_axis(pm, a)) / ((fp - fm) * length); + } + /* the joints at the sample, seeded from the sample before so the + whole segment stays on one solution branch, then the Jacobian + on that branch */ + if (kinematicsUserInverse(ctx, &p, joints) != 0 + || kinematicsUserJacobianAt(ctx, joints, &p, J) != 0) { + out->unanswered++; + continue; + } + for (j = 0; j < njoints; j++) { + g[k][j] = 0.0; + h[k][j] = 0.0; + bulge[k][j] = 0.0; + for (a = 0; a < AXIS_COUNT; a++) { g[k][j] += J[j][a] * t[a]; } + } + ok[k] = true; + answered++; + } + if (!answered) { return -1; } + + /* how far the rate can rise between two samples: the parabola + through three in a row bulges by an eighth of its second + difference over its middle, and a peak between samples is + covered by charging that to all three */ + for (k = 1; k + 1 < K; k++) { + if (!ok[k - 1] || !ok[k] || !ok[k + 1]) { continue; } + for (j = 0; j < njoints; j++) { + double b = fabs(g[k + 1][j] - 2.0 * g[k][j] + g[k - 1][j]) / 8.0; + if (b > bulge[k][j]) { bulge[k][j] = b; } + if (b > bulge[k - 1][j]) { bulge[k - 1][j] = b; } + if (b > bulge[k + 1][j]) { bulge[k + 1][j] = b; } + } + } + + /* the joint velocity limits */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + double rate = fabs(g[k][j]) + bulge[k][j]; + if (rate > tiny && lim->vel[j] / rate < out->vel) { + out->vel = lim->vel[j] / rate; + out->vel_joint = j; + out->vel_at = s[k] / length; + } + } + } + + /* how the rates change along the segment, read between neighbours + and charged to both */ + for (prev = -1, k = 0; k < K; k++) { + if (!ok[k]) { continue; } + if (prev >= 0) { + for (j = 0; j < njoints; j++) { + double dh = fabs(g[k][j] - g[prev][j]) / (s[k] - s[prev]); + if (dh > h[k][j]) { h[k][j] = dh; } + if (dh > h[prev][j]) { h[prev][j] = dh; } + } + } + prev = k; + } + + /* the changing rate at the capped velocity takes at most half a + joint's acceleration budget */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + if (h[k][j] > tiny) { + double cap = sqrt(lim->acc[j] / (2.0 * h[k][j])); + if (cap < out->vel) { + out->vel = cap; + out->vel_joint = j; + out->vel_at = s[k] / length; + } + } + } + } + + /* the joint acceleration limits, less what the changing rate takes */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + double rate = fabs(g[k][j]) + bulge[k][j]; + if (rate > tiny) { + double cap = (lim->acc[j] - h[k][j] * out->vel * out->vel) / rate; + if (cap < out->acc) { + out->acc = cap; + out->acc_joint = j; + } + } + } + } + return 0; +} // segmentCap() + +} // namespace motion_planning diff --git a/src/emc/motion_planning/segment_cap.hh b/src/emc/motion_planning/segment_cap.hh new file mode 100644 index 00000000000..80826bcb900 --- /dev/null +++ b/src/emc/motion_planning/segment_cap.hh @@ -0,0 +1,75 @@ +/******************************************************************** + * Description: segment_cap.hh + * The velocity and acceleration a segment can be run at without + * asking any joint for more than its own limits, read off the + * kinematics module's Jacobian along the segment. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + ********************************************************************/ +#ifndef SEGMENT_CAP_HH +#define SEGMENT_CAP_HH + +#include +#include + +namespace motion_planning { + +/* The joint limits the cap is read against, in the units motion commands + the joints in: [JOINT_n] MAX_VELOCITY and MAX_ACCELERATION. */ +struct SegmentCapLimits { + int joints; + double vel[KINEMATICS_USER_MAX_JOINTS]; + double acc[KINEMATICS_USER_MAX_JOINTS]; +}; + +/* No joint binds. */ +#define SEGMENT_CAP_NONE 1e9 + +/* The most a segment was sampled at. */ +#define SEGMENT_CAP_MAX_SAMPLES 129 + +/* What the joints allow along the segment: the fastest the path parameter + may advance, and the hardest it may accelerate, per second. */ +struct SegmentCap { + double vel; + double acc; + int vel_joint; /* the joint setting each, -1 for none */ + int acc_joint; + double vel_at; /* the fraction of the segment the velocity cap is read at */ + int samples; /* points the segment was sampled at */ + int unanswered; /* of them, the ones the module could not invert or differentiate */ +}; + +/* The pose at a fraction of the segment, 0 the start and 1 the end, in + the units the module takes, which are motion's. A line interpolates, + an arc walks its circle. */ +typedef void (*SegmentPoseFn)(double fraction, EmcPose *pose, void *arg); + +/* Sample the segment at `samples` points, the ends among them, and read + at each the rate every joint moves at per unit of path parameter: the + module's Jacobian along the local tangent. Between neighbouring samples + read how that rate changes with the parameter, and from three in a row + how much it can bulge between two of them, which is added to the rate so + that a peak falling between samples is covered. A joint's velocity + limit then caps the path velocity, and its acceleration limit caps the + path acceleration once the share the changing rate takes at the capped + velocity (what a carriage on a circle feels as centripetal) is set + aside: at most half the joint's budget goes to it, and the velocity is + lowered where it would take more. + + length is the segment's extent in the path parameter, in the units the + caps come back in. joints seeds the inverse at the start and comes back + holding the joints at the end, so the next segment starts on the same + solution branch. Returns 0, or -1 where no sample could be evaluated, + the caps then NONE. */ +int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int samples, SegmentPoseFn pose_at, void *arg, + double *joints, SegmentCap *out); + +} // namespace motion_planning + +#endif // SEGMENT_CAP_HH diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 2037aba3a62..18ae2bc78cb 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -314,6 +314,8 @@ extern int emcJointSetHomingParams(int joint, double home, double offset, double extern int emcJointUpdateHomingParams(int joint, double home, double offset, int sequence); extern int emcJointSetMaxVelocity(int joint, double vel); extern int emcJointSetMaxAcceleration(int joint, double acc); +extern double emcJointGetMaxVelocity(int joint); +extern double emcJointGetMaxAcceleration(int joint); extern int emcJointInit(int joint); extern int emcJointHalt(int joint); diff --git a/src/emc/task/Submakefile b/src/emc/task/Submakefile index 9fdab7004d6..e775d36d7af 100644 --- a/src/emc/task/Submakefile +++ b/src/emc/task/Submakefile @@ -32,7 +32,7 @@ USERSRCS += $(MILLTASKSRCS) ../bin/milltask: $(call TOOBJS, $(MILLTASKSRCS)) ../lib/librs274.so.0 ../lib/liblinuxcnc.a \ ../lib/libnml.so.0 ../lib/liblinuxcncini.so.1 ../lib/libposemath.so.0 \ ../lib/liblinuxcnchal.so.0 ../lib/libpyplugin.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libkinslimits.so.0 $(ECHO) Linking $(notdir $@) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index c2d96933119..a7bff9e7d24 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -69,6 +69,11 @@ #include "tooldata/tooldata.hh" #include #include +#include // getpid() +#include +#include +#include +#include using namespace linuxcnc; @@ -486,6 +491,13 @@ static double toExtVel(double vel) { static double toExtAcc(double acc) { return toExtVel(acc); } +static double fromExtVel(double vel) { + if (!canon.cartesian_move && canon.angular_move) { + return FROM_EXT_ANG(vel); + } + return FROM_EXT_LEN(vel); +} + static void send_g5x_msg(int index) { flush_segments(); @@ -840,6 +852,306 @@ static double getStraightJerk(double x, double y, double z, return 0.0; // a move to nowhere } +//---------------------------------------------------------------------- +// The kinematics ahead of motion: what the joints allow a segment. +// +// A segment is planned in world coordinates against the [AXIS_L] +// velocity and acceleration limits, which say nothing about the joints +// once the kinematics is not the identity: a turn of a tilted head under +// the tool centre point swings a carriage through a circle the head's +// own limit never mentions. So every segment is also sampled through +// the module's Jacobian here, on the kinematics type and the tool offset +// the program is under (canon runs ahead of motion, so neither is what +// the machine has reached), and its velocity and acceleration are +// lowered to what the slowest joint can follow. The module is the one +// motion runs, loaded through the non-RT loader in kinematics_userspace/ +// the first time a segment asks. A module that runs only in realtime, +// and a kinematics type that is the identity, cap nothing. +//---------------------------------------------------------------------- + +static struct { + KinematicsUserContext *ctx; + int comp_id; + bool tried; // loading was attempted, once + int type; // the kinematics type the program is in, -1 for the machine's + double joints[EMCMOT_MAX_JOINTS]; // the joints at canon.endPoint, as far as canon knows + bool in_arc; // ARC_FEED caps the arc itself; the straight helpers stand back + bool debug; // EMCCANON_KINS_DEBUG in the environment: say what every segment got + motion_planning::SegmentCapLimits limits; + // the last straight segment's answer, since its velocity and its + // acceleration are asked for separately + struct { + bool valid; + EmcPose start, end; + bool capped; + double vel, acc; + } memo; +} kins = { NULL, -1, false, -1, {}, false, false, {}, { false, {}, {}, false, 0.0, 0.0 } }; + +// the machine's own module, once +static bool kins_load(void) +{ + char name[HAL_NAME_LEN + 1]; + int joints, j; + + if (kins.tried) { return kins.ctx != NULL; } + kins.tried = true; + linuxcnc::IniFile ini(emc_inifile); + if (!ini) { return false; } + auto module = ini.findString("KINEMATICS", "KINS"); + joints = ini.findIntV("JOINTS", "KINS", 0); + if (!module || joints < 1) { return false; } + snprintf(name, sizeof(name), "canon.%d", (int)getpid()); + kins.comp_id = hal_init(name); + if (kins.comp_id < 0) { return false; } + kins.ctx = kinematicsUserInitString(module->c_str(), joints, kins.comp_id, name); + hal_ready(kins.comp_id); + if (kins.ctx && kinematicsUserIsRtOnly(kins.ctx)) { + kinematicsUserFree(kins.ctx); + kins.ctx = NULL; + } + if (!kins.ctx) { + hal_exit(kins.comp_id); + kins.comp_id = -1; + return false; + } + // the joint limits as the INI gives them; one left out binds nothing + kins.limits.joints = kinematicsUserGetNumJoints(kins.ctx); + for (j = 0; j < KINEMATICS_USER_MAX_JOINTS; j++) { + double vel = j < kins.limits.joints ? emcJointGetMaxVelocity(j) : 0.0; + double acc = j < kins.limits.joints ? emcJointGetMaxAcceleration(j) : 0.0; + kins.limits.vel[j] = vel > 0.0 ? vel : SEGMENT_CAP_NONE; + kins.limits.acc[j] = acc > 0.0 ? acc : SEGMENT_CAP_NONE; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { kins.joints[j] = 0.0; } + kins.debug = getenv("EMCCANON_KINS_DEBUG") != NULL; + if (kins.debug) { + fprintf(stderr, "canon kins: %s, %d joints, limits", module->c_str(), kins.limits.joints); + for (j = 0; j < kins.limits.joints; j++) { + fprintf(stderr, " %g/%g", kins.limits.vel[j], kins.limits.acc[j]); + } + fprintf(stderr, "\n"); + } + return true; +} + +// the module on the type and the tool offset the program is under, or +// NULL where there is nothing to cap with +static KinematicsUserContext *kins_here(void) +{ + int type = kins.type >= 0 ? kins.type : GET_EXTERNAL_KINS_TYPE(); + int flags = GET_EXTERNAL_KINS_TYPE_FLAGS(type); + EmcPose tool; + + // the identity, whether the module as a whole is one or the type in + // force is, needs no module loaded to know the joints are the axes + if (GET_EXTERNAL_KINEMATICS_IDENTITY()) { return NULL; } + if (flags >= 0 && (flags & KINSTYPE_IDENTITY)) { return NULL; } + if (!kins_load()) { return NULL; } + if (kinematicsUserSetType(kins.ctx, type) != 0) { return NULL; } + if (kinematicsUserIsIdentity(kins.ctx)) { return NULL; } + tool = to_ext_pose(canon.toolOffset.tran.x, canon.toolOffset.tran.y, canon.toolOffset.tran.z, + canon.toolOffset.a, canon.toolOffset.b, canon.toolOffset.c, + canon.toolOffset.u, canon.toolOffset.v, canon.toolOffset.w); + kinematicsUserSetTool(kins.ctx, &tool); + return kins.ctx; +} + +// whether two machine points are the same, to a hair either way +static bool kins_same(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-6; + + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + +// the joints at the start of a segment, as far as canon can know ahead of +// motion: the ones it holds while they still explain the point, since a +// point does not name one joint set, else the joints the machine stands +// in if those do, else the inverse iterated to a fixed point +static void kins_seed(KinematicsUserContext *ctx, const EmcPose *start) +{ + double standing[EMCMOT_MAX_JOINTS] = {0}; + EmcPose seeded = *start; + int i, n, pass; + + if (kinematicsUserForward(ctx, kins.joints, &seeded) == 0 && kins_same(&seeded, start)) { return; } + n = GET_EXTERNAL_JOINT_POSITIONS(standing, EMCMOT_MAX_JOINTS); + if (n > 0) { + seeded = *start; + if (kinematicsUserForward(ctx, standing, &seeded) == 0 && kins_same(&seeded, start)) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { kins.joints[i] = standing[i]; } + return; + } + } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = kins.joints[i]; } + if (kinematicsUserInverse(ctx, start, kins.joints) != 0) { return; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(kins.joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } +} + +// how many points to sample a segment at: enough that a turn of the +// rotaries, a stretch of travel on a machine whose Jacobian changes with +// position, or a sweep of arc is seen every few degrees or centimetres +static int kins_samples(const EmcPose *start, const EmcPose *end, double sweep_deg) +{ + double rot = fmax(fabs(end->a - start->a), fmax(fabs(end->b - start->b), fabs(end->c - start->c))); + double lin = sqrt(pow(end->tran.x - start->tran.x, 2) + pow(end->tran.y - start->tran.y, 2) + + pow(end->tran.z - start->tran.z, 2)); + int n = 3 + (int)ceil(rot / 10.0) + (int)ceil(lin / 50.0) + (int)ceil(sweep_deg / 10.0); + + return n > SEGMENT_CAP_MAX_SAMPLES ? SEGMENT_CAP_MAX_SAMPLES : n; +} + +// what the joints allow along a segment, in the machine's units per second +// of the path parameter canon plans it with; false where nothing binds +static bool kins_cap(KinematicsUserContext *ctx, const EmcPose *start, const EmcPose *end, + double length, int samples, motion_planning::SegmentPoseFn pose_at, void *arg, + double *vel, double *acc) +{ + motion_planning::SegmentCap cap; + + kins_seed(ctx, start); + if (motion_planning::segmentCap(ctx, &kins.limits, length, samples, pose_at, arg, kins.joints, &cap) != 0) { + CANON_ERROR(fmt::format("the kinematics cannot reach the move to X{:.3f} Y{:.3f} Z{:.3f} A{:.3f} B{:.3f} C{:.3f}, the joints are not checked along it", + end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c)); + return false; + } + if (cap.unanswered) { + CANON_ERROR(fmt::format("the kinematics cannot reach {} of {} points on the move to X{:.3f} Y{:.3f} Z{:.3f} A{:.3f} B{:.3f} C{:.3f}, the joints are not checked there", + cap.unanswered, cap.samples, end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c)); + } + // a pole: some joint would have to move at any speed at all for the + // path to advance, and the move crawls rather than being dropped, + // since a dropped segment would send the next one along another path + if (cap.vel_joint >= 0 && cap.vel < 1e-3) { + CANON_ERROR(fmt::format("joint {} cannot follow the move to X{:.3f} Y{:.3f} Z{:.3f} A{:.3f} B{:.3f} C{:.3f} near {:.0f}% of its length: the kinematics is singular there and the move crawls", + cap.vel_joint, end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, 100.0 * cap.vel_at)); + } + if (kins.debug) { + fprintf(stderr, "canon kins: to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f length %.3f in %d samples: vel %.3f (joint %d at %.2f) acc %.3f (joint %d), %d unanswered; seed", + end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, length, cap.samples, + cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered); + for (int j = 0; j < kins.limits.joints; j++) { fprintf(stderr, " %.3f", kins.joints[j]); } + fprintf(stderr, "\n"); + } + *vel = cap.vel; + *acc = cap.acc; + return cap.vel_joint >= 0 || cap.acc_joint >= 0; +} + +struct KinsLine { + EmcPose start, end; +}; + +static void kins_line_pose(double f, EmcPose *pose, void *arg) +{ + const KinsLine *line = (const KinsLine *)arg; + + pose->tran.x = line->start.tran.x + f * (line->end.tran.x - line->start.tran.x); + pose->tran.y = line->start.tran.y + f * (line->end.tran.y - line->start.tran.y); + pose->tran.z = line->start.tran.z + f * (line->end.tran.z - line->start.tran.z); + pose->a = line->start.a + f * (line->end.a - line->start.a); + pose->b = line->start.b + f * (line->end.b - line->start.b); + pose->c = line->start.c + f * (line->end.c - line->start.c); + pose->u = line->start.u + f * (line->end.u - line->start.u); + pose->v = line->start.v + f * (line->end.v - line->start.v); + pose->w = line->start.w + f * (line->end.w - line->start.w); +} + +// the cap on a straight segment from canon.endPoint, in the machine's +// units per second of the parameter getStraightVelocity() plans it with, +// the length along the axes getStraightSpan() measures +static bool kins_straight_cap(double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double *vel, double *acc) +{ + KinematicsUserContext *ctx; + KinsLine line; + double length; + + if (kins.in_arc) { return false; } + line.start = to_ext_pose(canon.endPoint); + line.end = to_ext_pose(x, y, z, a, b, c, u, v, w); + if (kins.memo.valid && kins_same(&kins.memo.start, &line.start) && kins_same(&kins.memo.end, &line.end)) { + *vel = kins.memo.vel; + *acc = kins.memo.acc; + return kins.memo.capped; + } + ctx = kins_here(); + if (!ctx) { return false; } + // the length getStraightVelocity() plans the segment along + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); + if (!span.moving) { + return false; + } + length = axisKindsMeasuredAngular(kinds, span.measured) ? + TO_EXT_ANG(span.length) : TO_EXT_LEN(span.length); + kins.memo.valid = true; + kins.memo.start = line.start; + kins.memo.end = line.end; + kins.memo.capped = kins_cap(ctx, &line.start, &line.end, length, + kins_samples(&line.start, &line.end, 0.0), + kins_line_pose, &line, &kins.memo.vel, &kins.memo.acc); + *vel = kins.memo.vel; + *acc = kins.memo.acc; + return kins.memo.capped; +} + +struct KinsArc { + PmCircle circle; // in canon's units, as ARC_FEED builds it + bool line; // no turn at all: the chord + EmcPose start, end; +}; + +static void kins_arc_pose(double f, EmcPose *pose, void *arg) +{ + const KinsArc *arc = (const KinsArc *)arg; + PmCartesian p; + + kins_line_pose(f, pose, (void *)&arc->start); + if (arc->line) { return; } + pmCirclePoint(&arc->circle, f * arc->circle.angle, &p); + pose->tran.x = TO_EXT_LEN(p.x); + pose->tran.y = TO_EXT_LEN(p.y); + pose->tran.z = TO_EXT_LEN(p.z); +} + +// the cap on an arc from canon.endPoint, in the machine's units per +// second of the XYZ length ARC_FEED plans it with +static bool kins_arc_cap(const PM_CARTESIAN ¢er, const PM_CARTESIAN &normal, + const PM_CARTESIAN &end_xyz, const CANON_POSITION &endpt, + int rotation, double length, double full_angle, + double *vel, double *acc) +{ + KinematicsUserContext *ctx = kins_here(); + KinsArc arc; + + if (!ctx || length <= 0.0) { return false; } + arc.start = to_ext_pose(canon.endPoint); + arc.end = to_ext_pose(endpt); + arc.line = rotation == 0; + if (!arc.line) { + PmCartesian s = { canon.endPoint.x, canon.endPoint.y, canon.endPoint.z }; + PmCartesian e = { end_xyz.x, end_xyz.y, end_xyz.z }; + PmCartesian c = { center.x, center.y, center.z }; + PmCartesian n = { normal.x, normal.y, normal.z }; + if (pmCircleInit(&arc.circle, &s, &e, &c, &n, rotation > 0 ? rotation - 1 : rotation) != 0) { + return false; + } + } + return kins_cap(ctx, &arc.start, &arc.end, TO_EXT_LEN(length), + kins_samples(&arc.start, &arc.end, fabs(full_angle) * 180.0 / M_PI), + kins_arc_pose, &arc, vel, acc); +} + /** * Get the limiting acceleration for a displacement from the current position to the given position. * returns a single acceleration that is the minimum of all axis accelerations. @@ -869,6 +1181,14 @@ static AccelData getStraightAcceleration(double x, double y, double z, if (out.tmax > 0.0) { out.acc = out.dtot / out.tmax; } + // and what the joints allow, through the kinematics + { + double kvel, kacc; + if (out.acc > 0.0 && kins_straight_cap(x, y, z, a, b, c, u, v, w, &kvel, &kacc) + && fromExtVel(kacc) < out.acc) { + out.acc = fromExtVel(kacc); + } + } if(debug_velacc) printf("cartesian %d ang %d acc %g\n", canon.cartesian_move, canon.angular_move, out.acc); return out; @@ -913,6 +1233,14 @@ static VelData getStraightVelocity(double x, double y, double z, } else { out.vel = canon.linearFeedRate; } + // and what the joints allow, through the kinematics + { + double kvel, kacc; + if (out.vel > 0.0 && kins_straight_cap(x, y, z, a, b, c, u, v, w, &kvel, &kacc) + && fromExtVel(kvel) < out.vel) { + out.vel = fromExtVel(kvel); + } + } if(debug_velacc) printf("cartesian %d ang %d vel %g\n", canon.cartesian_move, canon.angular_move, out.vel); return out; @@ -1101,6 +1429,10 @@ void SELECT_KINS_TYPE(int switchkins_type) { flush_segments(); + // the segments from here on run under it + kins.type = switchkins_type; + kins.memo.valid = false; + auto selectKinsMsg = std::make_unique(); selectKinsMsg->switchkins_type = switchkins_type; @@ -2801,6 +3133,7 @@ void ARC_FEED(int line_number, // Find the equivalent maximum velocity for a linear displacement // This accounts for speed restrictions due to helical and other axes + kins.in_arc = true; VelData veldata = getStraightVelocity(endpt); // Compute spiral length, first by the minimum circular arc length @@ -2833,6 +3166,7 @@ void ARC_FEED(int line_number, // Use "straight" acceleration measure to compute acceleration bounds due // to non-circular components (helical axis, other axes) AccelData accdata = getStraightAcceleration(endpt); + kins.in_arc = false; double tt_max_motion = accdata.tmax; double tt_max_spiral = spiral_length / a_max_axes; @@ -2842,6 +3176,16 @@ void ARC_FEED(int line_number, // circle plane and helical axis will still be within limits double a_max = total_xyz_length / tt_max; + // and what the joints allow along the arc, through the kinematics + { + double kvel, kacc; + if (kins_arc_cap(center_cart, normal_cart, end_cart, endpt, rotation, + total_xyz_length, full_angle, &kvel, &kacc)) { + v_max = std::min(v_max, FROM_EXT_LEN(kvel)); + a_max = std::min(a_max, FROM_EXT_LEN(kacc)); + } + } + // Limit velocity by maximum double vel = std::min(canon.linearFeedRate, v_max); canon_debug("current F = %f\n",canon.linearFeedRate); @@ -3059,6 +3403,7 @@ static void use_tool_length_offset(const EmcPose& offset, const EmcPose *point) canon.toolOffset.u = FROM_PROG_AX(AXIS_U, offset.u); canon.toolOffset.v = FROM_PROG_AX(AXIS_V, offset.v); canon.toolOffset.w = FROM_PROG_AX(AXIS_W, offset.w); + kins.memo.valid = false; /* append it to interp list so it gets updated at the right time, not at read-ahead time */ @@ -3543,6 +3888,8 @@ void INIT_CANON() canon.linearFeedRate = 0.0; canon.angularFeedRate = 0.0; ZERO_EMC_POSE(canon.toolOffset); + kins.type = -1; + kins.memo.valid = false; { std::string err; @@ -3857,7 +4204,10 @@ 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 + // without the interpreter seeing it. The interpreter asks when it + // synchs to the machine; canon's own idea of the type goes with it + kins.type = -1; + kins.memo.valid = false; return emcStatus->motion.traj.switchkins_type; } diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index c291547cf39..e886366f78c 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -552,6 +552,24 @@ int emcAxisSetLockingJoint(int axis, int joint) return retval; } +double emcJointGetMaxVelocity(int joint) +{ + if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { + return 0; + } + + return JointConfig[joint].MaxVel; +} + +double emcJointGetMaxAcceleration(int joint) +{ + if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { + return 0; + } + + return JointConfig[joint].MaxAccel; +} + double emcAxisGetMaxVelocity(int axis) { if (axis < 0 || axis >= EMCMOT_MAX_AXIS) { diff --git a/tests/kins-limits/box/README b/tests/kins-limits/box/README new file mode 100644 index 00000000000..91b37563b61 --- /dev/null +++ b/tests/kins-limits/box/README @@ -0,0 +1,17 @@ +The [AXIS_L] box is the machine frame envelope: the carriage, not the +tool tip. + +5axiskins with the joints reaching 1000 and the box at 500, a 250 pivot and +a 50 tool, the head laid over to B90 so that the tip sits 300 from the +carriage in the XY plane. Under the tool centre point kinematics: a move +whose tip is past the box with the carriage inside it is accepted; a move +whose tip is inside with the carriage past the box is refused; G53.5 to a +carriage position past the box is refused, and to one inside it with the +tip past the box is accepted. A carriage past the joint limit is refused +whatever the box says. Nothing is asserted about the identity kinematics, +where the box and the joints describe the same thing. + +maxkins, one type, its joints the slides X to W: the same with a 250 pivot +and a 50 tool, the head laid over to B90 by a move that keeps the tip. The +module has no machine frame type to name, and reads the box on its joints +through the map. diff --git a/tests/kins-limits/box/checkresult b/tests/kins-limits/box/checkresult new file mode 100755 index 00000000000..8f4f3f58ba7 --- /dev/null +++ b/tests/kins-limits/box/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures, once per machine +[ "$(grep -c "^Exiting with 0 errors" "$1")" = 2 ] diff --git a/tests/kins-limits/box/maxkins.hal b/tests/kins-limits/box/maxkins.hal new file mode 100644 index 00000000000..551dc8d422b --- /dev/null +++ b/tests/kins-limits/box/maxkins.hal @@ -0,0 +1,19 @@ +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 + +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 J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb +net J6 joint.6.motor-pos-cmd => joint.6.motor-pos-fb +net J7 joint.7.motor-pos-cmd => joint.7.motor-pos-fb +net J8 joint.8.motor-pos-cmd => joint.8.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-limits/box/maxkins.ini b/tests/kins-limits/box/maxkins.ini new file mode 100644 index 00000000000..04ce1e06a02 --- /dev/null +++ b/tests/kins-limits/box/maxkins.ini @@ -0,0 +1,180 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# one type, the tool centre point, its joints the slides X to W in order; +# the joints reach further than the [AXIS_L] box, so the box is a +# restriction the config imposes and not the travel +KINEMATICS = maxkins +JOINTS = 9 + +[HAL] +HALFILE = maxkins.hal +HALCMD = setp maxkins.pivot-length 250 + +[TRAJ] +# the rates are high so the moves are short: the test checks only where +# they end +COORDINATES = XYZABCUVW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 2000 +MAX_LINEAR_VELOCITY = 3460 +MAX_LINEAR_ACCELERATION = 80000 +DEFAULT_LINEAR_ACCELERATION = 80000 +MAX_ANGULAR_VELOCITY = 3600 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_A] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_U] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_V] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_6] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_7] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_8] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/box/sim.hal b/tests/kins-limits/box/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-limits/box/sim.hal @@ -0,0 +1,16 @@ +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 + +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 J5 joint.5.motor-pos-cmd => joint.5.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-limits/box/test-ui.py b/tests/kins-limits/box/test-ui.py new file mode 100755 index 00000000000..6fb0e193b51 --- /dev/null +++ b/tests/kins-limits/box/test-ui.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +# The [AXIS_L] box is the machine frame envelope, the carriage and not the +# tool tip: see README. +import linuxcnc +import sys +import time + +ini = linuxcnc.ini(sys.argv[2]) +MAX = ini.find("KINS", "KINEMATICS") == "maxkins" +JOINTS = int(ini.find("KINS", "JOINTS")) + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def accepted(cmd, carriage_x): + drain() + now = mdi(cmd) + said = [m for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s was refused: %s" % (cmd, said[0][1].strip())) + elif abs(now[0] - carriage_x) > 1e-3: + error("%s left the carriage at X %.3f, not %.3f" % (cmd, now[0], carriage_x)) + else: + print("accepted as expected: %-24s carriage X %.3f" % (cmd, now[0])) + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +def refused(cmd, needle): + drain() + before = joints() + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + settled() + error("%s was accepted" % cmd) + elif needle not in m[1]: + error("%s said %r, nothing about %r" % (cmd, m[1].strip(), needle)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + settled() + if max(abs(a - b) for a, b in zip(joints(), before)) > 1e-6: + error("%s moved the joints" % cmd) + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +if MAX: + # maxkins has one type, the tool centre point, its joints the slides: + # with the tool on, the head laid over to B90 by a move that keeps the + # tip, which leaves the carriage 300 from the tip along X + mdi("G43 H1") + mdi("G0 X0 Y0 Z0 B90") + j = joints() + s.poll() + d = s.position[0] - j[0] + print("carriage X %.3f, tip X %.3f" % (j[0], s.position[0])) + if abs(abs(d) - 300) > 1e-3: + error("the tip is %.3f from the carriage, not 300" % d) + side = 1 if d > 0 else -1 + accepted("G0 X%g" % (700 * side), 400 * side) + refused("G0 X%g" % (-300 * side), "limit") + refused("G0 X%g" % (-800 * side), "limit") + refused("G53.5 G0 X%g" % (-600 * side), "limit") + accepted("G53.5 G0 X%g" % (400 * side), 400 * side) + mdi("G0 X0 B0") + mdi("G49") +else: + # the tool centre point kinematics with the tool on, the head laid over by + # a joint move: the tip is now 300 from the carriage along +X at C0 + mdi("G12.1 P0") + mdi("G43 H1") + mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=90 J4=0 J5=0") + j = joints() + s.poll() + print("carriage X %.3f, tip X %.3f" % (j[0], s.position[0])) + if abs(s.position[0] - j[0] - 300) > 1e-3: + error("the tip is %.3f from the carriage, not 300" % (s.position[0] - j[0])) + + # the tip past the box, the carriage inside it: accepted + accepted("G0 X600", 300) + # the tip inside the box, the carriage past it: refused + refused("G0 X-300", "limit") + # the carriage past the joint limit: refused whatever the box says + refused("G0 X-800", "limit") + # G53.5 names the carriage: past the box refused, inside it accepted even + # with the tip past the box + refused("G53.5 G0 X-600", "limit") + accepted("G53.5 G0 X400", 400) + + mdi("G53.7 G0 J0=0 J3=0") + mdi("G49") +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/box/test.ini b/tests/kins-limits/box/test.ini new file mode 100644 index 00000000000..f3300215554 --- /dev/null +++ b/tests/kins-limits/box/test.ini @@ -0,0 +1,134 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the joints reach further than the [AXIS_L] box, so the box is a +# restriction the config imposes and not the travel +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +# the rates are high so the moves are short: the test checks only where +# they end +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 2000 +MAX_LINEAR_VELOCITY = 3460 +MAX_LINEAR_ACCELERATION = 80000 +DEFAULT_LINEAR_ACCELERATION = 80000 +MAX_ANGULAR_VELOCITY = 3600 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/box/test.sh b/tests/kins-limits/box/test.sh new file mode 100755 index 00000000000..e74944010c9 --- /dev/null +++ b/tests/kins-limits/box/test.sh @@ -0,0 +1,7 @@ +#!/bin/bash -e +for ini in test.ini maxkins.ini; do + # a failed run leaves the var file behind, and it carries offsets + rm -f sim.var sim.var.bak + echo "=== $ini" + linuxcnc -r $ini +done diff --git a/tests/kins-limits/box/tool.tbl b/tests/kins-limits/box/tool.tbl new file mode 100644 index 00000000000..b558277601f --- /dev/null +++ b/tests/kins-limits/box/tool.tbl @@ -0,0 +1 @@ +T1 P1 D6.0 Z50 ; diff --git a/tests/kins-limits/jog-travel/README b/tests/kins-limits/jog-travel/README new file mode 100644 index 00000000000..4a79b847ba6 --- /dev/null +++ b/tests/kins-limits/jog-travel/README @@ -0,0 +1,15 @@ +A world jog under the tool centre point slows to a stop where a joint +reaches its soft limit, as a jog of a slide does at its axis limit, and a +jog away from the limit is free. + +The jog test's machine (5axiskins, 400 mm pivot, the head laid over to +B90 by a joint move, the joints following through limit3) with joint 1 +given 300 mm of travel each way, so a jog of C with the tip held swings +the carriage out of it about 48.6 degrees round. In teleop mode: a +continuous jog of C held well past that, an incremental jog of 20 degrees +away, an incremental jog of 60 back towards the limit, and one away again. +The test asserts that each jog into the limit comes to rest with joint 1 +within 1 micron inside its limit and never past it, that the jog has +ended there and says nothing (no soft limit error, no following error), +that the jogs away land where they were sent, and that no joint's +commanded velocity or acceleration went over its INI limit. diff --git a/tests/kins-limits/jog-travel/checkresult b/tests/kins-limits/jog-travel/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/jog-travel/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/jog-travel/sim.hal b/tests/kins-limits/jog-travel/sim.hal new file mode 100644 index 00000000000..7c4e3727d8b --- /dev/null +++ b/tests/kins-limits/jog-travel/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 1920 +setp limit3.0.maxa 61440 +setp limit3.1.maxv 1920 +setp limit3.1.maxa 61440 +setp limit3.2.maxv 1920 +setp limit3.2.maxa 61440 +setp limit3.3.maxv 576 +setp limit3.3.maxa 15360 +setp limit3.4.maxv 576 +setp limit3.4.maxa 15360 +setp limit3.5.maxv 1920 +setp limit3.5.maxa 61440 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +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-limits/jog-travel/test-ui.py b/tests/kins-limits/jog-travel/test-ui.py new file mode 100755 index 00000000000..1d186a69eae --- /dev/null +++ b/tests/kins-limits/jog-travel/test-ui.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python3 +# A world jog under the tool centre point slows to a stop where a joint +# reaches its soft limit, and a jog away from it is free: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 +C = 5 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def checked(what, n0): + end = settled() + catch_up(end) + said = drain() + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + # a jog that stops at a joint's limit says nothing: no soft limit + # error, no following error + if said: + error("%s said: %s" % (what, said[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (what, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), what)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) + + +Y = 1 +YMAX = float(ini.find("JOINT_1", "MAX_LIMIT")) +YMIN = float(ini.find("JOINT_1", "MIN_LIMIT")) + +def at_limit(what, end): + # the carriage came to rest against its travel, not short of it and + # not past it + room = min(YMAX - end[Y], end[Y] - YMIN) + print("%s: joint 1 at %.6f, %.6f from its limit, C %.3f" % (what, end[Y], room, end[4])) + if room < 0: + error("%s: joint 1 at %.6f is past its limit" % (what, end[Y])) + elif room > 1e-3: + error("%s: joint 1 stopped %.6f short of its limit" % (what, room)) + +def jog_ended(what): + s.poll() + if any(s.axis[a]["velocity"] != 0 for a in range(9)): + error("%s: an axis is still moving" % what) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move; +# the carriage at X-400 puts the tip at X0, the centre of the circle a jog +# of C swings the carriage on. Joint 1 has 300 mm of travel each way, so +# the circle leaves it about 48.6 degrees round +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +drain() +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(30) +c.teleop_enable(1) +c.wait_complete(30) + +# a continuous jog of C held well past where joint 1 runs out: the jog +# slows and comes to rest with the carriage at its limit, and ends +n0 = catch_up(start) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, C, 480) +deadline = time.time() + 60 +moved = False +while time.time() < deadline: + s.poll() + if abs(s.position[C]) > 10: + moved = True + # held until the jog has come to rest with C well round, or a limit + # error has stopped it + if moved and s.axis[C]["velocity"] == 0: + break + if s.task_state != linuxcnc.STATE_ON: + break + time.sleep(0.01) +else: + error("the continuous jog of C did not come to rest") +c.jog(linuxcnc.JOG_STOP, 0, C) +end = checked("the continuous jog of C into the limit", n0) +at_limit("the continuous jog of C", end) +tip_held("the continuous jog of C") +jog_ended("the continuous jog of C") +stop_c = end[4] + +# away from the limit by 20 degrees: free, and it lands where it was sent +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, C, 480, -20 if stop_c > 0 else 20) +end = checked("the incremental jog of C away", n0) +want = stop_c - 20 if stop_c > 0 else stop_c + 20 +if abs(end[4] - want) > 1e-3: + error("C ended at %.3f, not %.3f, jogging away from the limit" % (end[4], want)) + +# an incremental jog of 60 degrees back towards it: it stops at the +# limit, short of where it was sent, and ends there +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, C, 480, 60 if stop_c > 0 else -60) +end = checked("the incremental jog of C into the limit", n0) +at_limit("the incremental jog of C", end) +tip_held("the incremental jog of C") +jog_ended("the incremental jog of C") + +# and a jog away works straight after it +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, C, 480, -10 if stop_c > 0 else 10) +again = checked("the jog away after the incremental one", n0) +want = end[4] - 10 if stop_c > 0 else end[4] + 10 +if abs(again[4] - want) > 1e-3: + error("C ended at %.3f, not %.3f, jogging away again" % (again[4], want)) + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/jog-travel/test.ini b/tests/kins-limits/jog-travel/test.ini new file mode 100644 index 00000000000..3a8e4293456 --- /dev/null +++ b/tests/kins-limits/jog-travel/test.ini @@ -0,0 +1,146 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +# every rate is eight times a real machine's, the accelerations 64 times, +# so the same moves take an eighth of the time +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 1600 +MAX_LINEAR_VELOCITY = 2768 +MAX_LINEAR_ACCELERATION = 51200 +DEFAULT_LINEAR_ACCELERATION = 51200 +MAX_ANGULAR_VELOCITY = 2880 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -300 +MAX_LIMIT = 300 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/jog-travel/test.sh b/tests/kins-limits/jog-travel/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/jog-travel/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/jog-travel/tool.tbl b/tests/kins-limits/jog-travel/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/jog-travel/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; diff --git a/tests/kins-limits/jog/README b/tests/kins-limits/jog/README new file mode 100644 index 00000000000..2f2e00310c9 --- /dev/null +++ b/tests/kins-limits/jog/README @@ -0,0 +1,17 @@ +A world jog under the tool centre point must not run a slide past its +own limits. + +The swing test's machine: 5axiskins with a 400 mm pivot, the head laid +over to B90 by a joint move, the joints following their commands through +limit3 at 20 percent over each joint's own limits, the way a drive or a +step generator follows only as fast as it can. Then, in teleop mode, a +continuous jog of C at the axis's full rate, stopped by the test past +150 degrees, an incremental jog of C back by 90, and an incremental jog +of X with the head still laid over. The tip holds through the jogs of +C while the carriage swings on a circle of radius 400; nothing about the +[AXIS_C] limits says how fast that is. The test asserts that no jog +trips a following error, that the incremental jogs land where they were +sent, and that no joint's commanded velocity or acceleration went over +the joint's INI limit, sampled every servo cycle. The pace of the jog +is not asserted: it is whatever the slowest joint allows, and it changes +along the swing. diff --git a/tests/kins-limits/jog/checkresult b/tests/kins-limits/jog/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/jog/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/jog/sim.hal b/tests/kins-limits/jog/sim.hal new file mode 100644 index 00000000000..7c4e3727d8b --- /dev/null +++ b/tests/kins-limits/jog/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 1920 +setp limit3.0.maxa 61440 +setp limit3.1.maxv 1920 +setp limit3.1.maxa 61440 +setp limit3.2.maxv 1920 +setp limit3.2.maxa 61440 +setp limit3.3.maxv 576 +setp limit3.3.maxa 15360 +setp limit3.4.maxv 576 +setp limit3.4.maxa 15360 +setp limit3.5.maxv 1920 +setp limit3.5.maxa 61440 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +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-limits/jog/test-ui.py b/tests/kins-limits/jog/test-ui.py new file mode 100755 index 00000000000..e863759c0e8 --- /dev/null +++ b/tests/kins-limits/jog/test-ui.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +# A world jog under the tool centre point must not run a slide past its +# own limits: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 +C = 5 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def checked(what, n0): + end = settled() + catch_up(end) + said = drain() + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + faults = [m for m in said if "following error" in m[1]] + if faults: + error("%s tripped a following error: %s" % (what, faults[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (what, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), what)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move so +# that getting there cannot swing anything; the carriage at X-400 puts the +# tip at X0, the centre of the circle a jog of C is about to swing it on +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), + " ".join("%.3f" % v for v in tip))) +drain() + +# world jogs: teleop mode +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(30) +c.teleop_enable(1) +c.wait_complete(30) + +# a continuous jog of C at the axis's full rate, the tip held: stopped by +# the test once C is past 150, so the planner ramps up, cruises, ramps down +n0 = catch_up(start) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, C, 480) +deadline = time.time() + 60 +while time.time() < deadline: + s.poll() + if s.position[C] > 150: + break + time.sleep(0.01) +c.jog(linuxcnc.JOG_STOP, 0, C) +end = checked("the continuous jog of C", n0) +if end[4] < 150: + error("C stopped at %.3f, before 150" % end[4]) +tip_held("the continuous jog of C") + +# an incremental jog back by 90 at the same rate: the planner knows its +# endpoint, the cap still holds +n0 = catch_up(end) +back_from = end[4] +c.jog(linuxcnc.JOG_INCREMENT, 0, C, -480, 90) +end = checked("the incremental jog of C", n0) +if abs(end[4] - (back_from - 90)) > 1e-3: + error("C ended at %.3f, not %.3f" % (end[4], back_from - 90)) +tip_held("the incremental jog of C") + +# a jog of X with the head still laid over: the Jacobian does not change +# along it, the carriage simply runs at the jog rate +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, 0, 1600, 100) +end = checked("the incremental jog of X", n0) + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/jog/test.ini b/tests/kins-limits/jog/test.ini new file mode 100644 index 00000000000..13379eb18a0 --- /dev/null +++ b/tests/kins-limits/jog/test.ini @@ -0,0 +1,146 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +# every rate is eight times a real machine's, the accelerations 64 times, +# so the same moves take an eighth of the time +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 1600 +MAX_LINEAR_VELOCITY = 2768 +MAX_LINEAR_ACCELERATION = 51200 +DEFAULT_LINEAR_ACCELERATION = 51200 +MAX_ANGULAR_VELOCITY = 2880 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/jog/test.sh b/tests/kins-limits/jog/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/jog/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/jog/tool.tbl b/tests/kins-limits/jog/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/jog/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; diff --git a/tests/kins-limits/swing/README b/tests/kins-limits/swing/README new file mode 100644 index 00000000000..a9d17309b6d --- /dev/null +++ b/tests/kins-limits/swing/README @@ -0,0 +1,15 @@ +A turn of the head under the tool centre point must not run a slide +past its own limits. + +5axiskins with a 400 mm pivot, the head laid over to B90 by a joint move, +then a plain G0 C180 with the tool tip held: the tip does not move, the +carriage swings through a half circle of radius 400 to keep it still. The +joints follow their commands through limit3, 20 percent over each joint's +own velocity and acceleration limits, the way a drive or a step generator +follows only as fast as it can; a command inside the limits is followed +exactly, one well over them falls behind and motion trips the following +error. The test asserts that the move ends on its endpoint with no +following error, and that no joint's commanded velocity or acceleration +went over the joint's INI limit, sampled every servo cycle. The slowest +joint has to set the pace of the move; how slow the tip goes is not +asserted. diff --git a/tests/kins-limits/swing/checkresult b/tests/kins-limits/swing/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/swing/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/swing/sim.hal b/tests/kins-limits/swing/sim.hal new file mode 100644 index 00000000000..7c4e3727d8b --- /dev/null +++ b/tests/kins-limits/swing/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 1920 +setp limit3.0.maxa 61440 +setp limit3.1.maxv 1920 +setp limit3.1.maxa 61440 +setp limit3.2.maxv 1920 +setp limit3.2.maxa 61440 +setp limit3.3.maxv 576 +setp limit3.3.maxa 15360 +setp limit3.4.maxv 576 +setp limit3.4.maxa 15360 +setp limit3.5.maxv 1920 +setp limit3.5.maxa 61440 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +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-limits/swing/test-ui.py b/tests/kins-limits/swing/test-ui.py new file mode 100755 index 00000000000..3a84f139ab4 --- /dev/null +++ b/tests/kins-limits/swing/test-ui.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +# A turn of the head under the tool centre point must not run a slide past +# its own limits: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move so +# that getting there cannot swing anything; the carriage at X-400 puts the +# tip at X0, the centre of the half circle it is about to draw +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), + " ".join("%.3f" % v for v in tip))) +drain() +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def swing(cmd, n0): + c.mdi(cmd) + c.wait_complete(60) + end = settled() + catch_up(end) + said = drain() + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + faults = [m for m in said if "following error" in m[1]] + if faults: + error("%s tripped a following error: %s" % (cmd, faults[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (cmd, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), cmd)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) + +# the swing: the tip holds, C turns half a revolution, the carriage follows +# a half circle of radius 400 to keep the tip where it is +end = swing("G0 C180", catch_up(start)) +if abs(end[4] - 180) > 1e-3: + error("C ended at %.4f, not 180" % end[4]) +tip_held("G0 C180") + +# the same on an arc: the tip draws a full circle of radius 100 at a feed +# it could keep on its own while C turns another revolution, so the +# carriage rides the small circle and the big swing together +end = swing("G17 G2 X0 Y0 I100 J0 C540 F40000", catch_up(end)) +if abs(end[4] - 540) > 1e-3: + error("C ended at %.4f, not 540" % end[4]) +tip_held("the arc") + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/swing/test.ini b/tests/kins-limits/swing/test.ini new file mode 100644 index 00000000000..13379eb18a0 --- /dev/null +++ b/tests/kins-limits/swing/test.ini @@ -0,0 +1,146 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +# every rate is eight times a real machine's, the accelerations 64 times, +# so the same moves take an eighth of the time +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 1600 +MAX_LINEAR_VELOCITY = 2768 +MAX_LINEAR_ACCELERATION = 51200 +DEFAULT_LINEAR_ACCELERATION = 51200 +MAX_ANGULAR_VELOCITY = 2880 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/swing/test.sh b/tests/kins-limits/swing/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/swing/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/swing/tool.tbl b/tests/kins-limits/swing/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/swing/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ;