Skip to content

kinematics: the limits mean the joints and the carriage, whatever kinematics is in force - #4842

Open
grandixximo wants to merge 8 commits into
LinuxCNC:masterfrom
grandixximo:kins-limits-pr
Open

grandixximo wants to merge 8 commits into
LinuxCNC:masterfrom
grandixximo:kins-limits-pr

Conversation

@grandixximo

Copy link
Copy Markdown
Contributor

Item 6 of the multiaxis plan in #4374. On a kinematics that is not the identity, the [AXIS_L] limits were checked on the world of the type in force, under TCP the tool tip, and nothing kept the joints within their own velocity and acceleration limits:

  • The [AXIS_L] box is now the machine frame envelope, the carriage, read through the forward of the module's machine frame type. Under TCP, a move with the tip past the box and the carriage inside is accepted, and one with the tip inside and the carriage past is refused. G53.5 gets the box too. maxkins, a single type module, says its joints are its slides so it gets the same box.
  • Every segment is capped at what its joints can follow. Canon samples each line and arc through the module's Jacobian and lowers its velocity and acceleration to what the slowest joint allows. Turning a tilted head under TCP no longer trips the carriage's following error.
  • A world jog is capped the same way in motion, every servo cycle. It also slows to rest where a joint reaches its soft limit and ends there without an error, as a jog of a slide does at its axis limit (Sigma1912 reported the hard stop).
  • Docs, the qtvcp MDI help and the hal_glib modal tables list every code the kinematics work added.

Four tests under tests/kins-limits, each red without its fix:

  • box: TCP and maxkins.
  • swing: a G0 C180 and an arc with the tip held, joints following through limit3 at 20 percent over their limits.
  • jog: the same machine in teleop.
  • jog-travel: a jog into a joint's soft limit and away.

The dynamic tests run their machine eight times faster than a real one so each takes under 7 s. They pass with 6 copies in parallel alongside 4 busy loops, and on one CPU.

…dentity

Two tests that fail today and define item 6 of the multiaxis plan: travel is the joint limits, on every move and every kinematics type, and the [AXIS_L] box is the machine frame envelope, the carriage, not the tool tip.

tests/kins-limits/swing: 5axiskins with a 400 mm pivot, the head laid over to B90 by a joint move, then G0 C180 with the tool tip held. The carriage draws a half circle of radius 400 to keep the tip still, and the planner runs C at C's own limits, so the slides are asked for 1396 mm/s^2 against their 800 and would be asked for 419 mm/s against their 200. The joints follow 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, so the command a joint cannot follow is a real following error: joint 1 trips it 84 cycles in. The test asserts no following error, the endpoint reached, and every joint's commanded velocity and acceleration within its own INI limit at every servo cycle, read from a sampler. The slowest joint has to set the pace; how fast the tip goes is not asserted.

tests/kins-limits/box: joints reaching 1000, the box at 500, a 250 pivot and a 50 tool with the head at B90 so that the tip sits 300 from the carriage. Under the tool centre point kinematics a move with the tip past the box and the carriage inside it must be accepted, one with the tip inside and the carriage past the box refused, G53.5 to a carriage past the box refused and to one inside it accepted whatever the tip does; a carriage past the joint limit is refused whatever the box says. Today the box is checked on the world of the kinematics in force, the tip, and never on a joint-target move, so three of the five go the wrong way.
…cs is in force

The position limits in [AXIS_L] were checked on the world of the kinematics in force, so under a tool centre point kinematics they boxed the tool tip, which moves with the tool length and the head, and a joint-target move never saw them at all. A box on the tip refuses moves the machine can make and lets through moves it cannot, which is why switchkins.adoc told configurations to setp ini.L.min_limit at every switch and the puma sims shipped M128 to M130 to do it.

The box is now the machine frame envelope: the carriage, or a robot's flange, in machine coordinates, the frame that does not move when a head tilts or a tool is loaded. kinematicsMachineFrame() runs the forward of the type flagged KINSTYPE_MACHINE on a set of joints without switching to it, and motion's inRange() takes every endpoint through it after the inverse, Cartesian and joint interpolated moves alike, so G0 and G53.5 to the same carriage position get the same answer. A module that declares no machine frame type keeps the box on the world of the type in force, as before; motion links the new entry point weakly so an older module still loads. The travel itself is the joint limits, checked on every move as they were.

The puma M-codes keep swapping the velocity and acceleration limits per type and leave the box alone, and M128 restores max_velocity rather than a pin that never existed. tests/kins-limits/box goes green: under the tool centre point a move with the tip past the box and the carriage inside is accepted, one with the tip inside and the carriage past the box is refused, and G53.5 gets the box too.
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 the carriage through a circle the head's own limit never mentions, and the carriage trips its following error.

Every segment is now also sampled through the kinematics module's Jacobian in canon, ahead of motion, on the kinematics type 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. motion_planning/segment_cap.cc does the reading: at each sample the rate of every joint per unit of path parameter along the local tangent; between samples how that rate changes, charged against the acceleration budget at the capped velocity (what a carriage on a circle feels as centripetal, at most half the budget, the velocity lowered where it would take more); from three samples in a row how much the rate can bulge between two of them, so a peak falling between samples is covered. Lines are sampled from canon.endPoint to the target, traverse, feed, probe, rigid tap and the chained segments of the naive cam alike, through getStraightVelocity() and getStraightAcceleration(); arcs are sampled on the posemath circle ARC_FEED builds, in ARC_FEED itself.

Canon owns its non-RT context: the module and joint count from [KINS] in the INI, the type tracked through SELECT_KINS_TYPE and dropped to the machine's whenever the interpreter synchs, the tool from the offset canon applies, the joints at the start of a segment kept from the last one while they still explain the point, else the machine's, else the inverse iterated to a fixed point. The identity, as a module or as the type in force, loads nothing. A module that runs only in realtime caps nothing, as before. Where the module cannot answer at a sample, or a joint would have to move at any speed at all (a pole), an operator message says so and the segment goes through capped rather than dropped, since a dropped segment sends the next one along another path.

tests/kins-limits/swing goes green, and gains a second phase: the tip draws a full circle 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. kinslimits prints the same cap for a move after its own per-sample listing. emcJointGetMaxVelocity() and emcJointGetMaxAcceleration() read back what iniJoint() set. The switchkins chapter's limit section says what the velocity and acceleration limits now mean and when a switch still wants them reset; the [AXIS_L] entry in the INI chapter points to it.
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, and the carriage trips its following error a fraction of a degree in. The segment cap in canon cannot help, a jog never passes canon.

Each servo cycle in teleop mode, before the axis planners run, teleop_joint_cap() reads the module's Jacobian at the joints the machine stands in (weak, so an old module without one keeps today's behaviour, and nothing is read on the identity) and, for the jog the planners were asked for, the rate every joint would move at per unit of it and how that rate has changed since the last cycle, what a carriage on a circle feels as centripetal. The planners are then 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 of it; the acceleration by what the budget leaves. Every active axis scales by the same factor, so a jog of two axes keeps its direction. The jog's own request is kept apart from the planner's caps in the axis, teleop_vel_req and teleop_acc_req, and axis_teleop_request() and axis_teleop_cap() are the two calls the cap needs from axis.c.

tests/kins-limits/jog is the swing test's machine in teleop mode: a continuous jog of C at the axis's full rate stopped past 150 degrees, an incremental jog back by 90, an incremental jog of X with the head still laid over, every joint following through limit3 at 20 percent over its limits; red before this (joint 1 asked for 1396 mm/s^2 against 800, following error at C 0.4), green with it, the carriage held at its limits and the jog of C running faster where the carriage rate is small and slowing as it grows. The switchkins and INI chapters say that a jog is bounded the same way as a move.
The quick reference, the qtvcp MDI help and the modal group tables of hal_glib knew none of G12.1, G13.1, G28.5, G30.5, G43.4, G43.5, G53.1 to G53.7, G68.2 to G68.4 and G69. One sweep for all of them, the same three places G28.2 was added to.

docs/src/gcode.html.in gets a row per code, linked to its section; the point-to-point and orientation codes sit under Motion, the two tool length forms under Tool Length Offset, the plane, its cancel and the kinematics selection under Other Modal Codes, the machine frame forms of G28 and G30 next to them. lib/python/qtvcp/lib/mdi_text.py gets a title, a word list and a description for each, condensed from the G-code chapter, so the qtvcp G-code help lists and explains them. lib/python/common/hal_glib.py gets the codes in their modal groups and a gcode-group9-changed signal for the tilted work plane, which stat.gcodes reports in its own slot; group 8 also gains G43.2, which was missing.

The allocated G-code tables of the remap chapter get the same codes, and row 33, which listed G30 and G30.1 in G33's place, now lists G33 and G33.1. The G28.5 heading and its cross-reference from G30 said slide position; it is the machine frame position, as the section itself says.
kinematicsUserJacobian() runs the module's inverse at the pose before differentiating, to be on the caller's solution branch. The segment sampler in canon has just run that inverse itself at every sample, so each sample paid for it twice. kinematicsUserJacobianAt() takes the joints the caller holds and differentiates there; the old entry inverts and then calls it. segmentCap() uses the new one, and its answers are unchanged.
The box is the machine frame envelope, read on the forward of the type a module flags as its machine frame. A module with one type, built on kins_single.c, flags none, and kept the box on the world of the type in force. On maxkins that is the tool tip, so the box moved with the tool length and the head: with the head laid over a move whose carriage stayed inside the box was refused, and one that drove the carriage past it was accepted, the case the box moved to the machine frame for.

A type may now say its joints are its slides, each in the letter the joint map gives it, and kins_single.c then answers the machine frame with the joints read through the map. maxkins says so. The other single-type modules keep the world of the type in force: a delta's or a tripod's joints are not slides along X Y Z, and corexy's are motors that drive them together.

tests/kins-limits/box runs maxkins as well, with a 250 pivot, a 50 tool and the head laid over to B90: the tip past the box with the carriage inside it is accepted, the tip inside with the carriage past it refused, G53.5 boxed on the carriage. Without the flag five of the six checks fail.
A world jog is planned per axis against the [AXIS_L] limits. Under the tool centre point a jog of a rotary moves the slides through the inverse, and nothing looked at their travel until a joint had crossed it: motion then reported the soft limit, stopped the jog hard and left the machine on the limit, to be jogged off in joint mode (reported by Sigma1912).

teleop_joint_cap() already reads, every servo cycle, how fast each joint moves per unit of the jog and caps the planners at the joints' velocity and acceleration limits. It now also caps them at what lets a joint heading for a soft limit stop at it, at half the acceleration the planners are left, and go no further than it in one cycle. The room is read from where the planners have taken the joint, the latest inverse, not from the interpolated command, which follows a few cycles behind and let the carriage run on past the limit. The jog stops a ten-millionth of the joint's range short, for the curve of the path over the last step, and a jog that has come to rest there ends, as a jog of a slide ends at its axis limit, with no error; a jog the other way moves off it. Siemens behaves the same way in JOG: the axis stops at its software limit switch (alarm 10621, axis rests on software limit switch) and is jogged off without a mode change, and under a transformation the axis that stops can be another than the one jogged.

tests/kins-limits/jog-travel is the jog test's machine with joint 1 given 300 mm each way, so a jog of C with the tip held swings the carriage out of its travel 48.6 degrees round: a continuous jog held well past that, an incremental jog away, one of 60 degrees back towards the limit and one away again. Before this the carriage ran 14.5 mm past its limit, the soft limit error stopped the jog, the hard stop asked joint 0 for 93000 mm/s^2 and the machine went off; with it each jog into the limit rests within 1 micron inside it, says nothing, and keeps every joint within its velocity and acceleration limits, and the jogs away land where they were sent.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant