Need for per joint homing behaviour

ArcEye commented 6 years ago

Issue by luminize Fri Jan 15 09:34:03 2016 Originally opened as https://github.com/machinekit/machinekit/issues/862

We need to have a component which can take care of homing a joint from a very minimal HAL setup.

https://github.com/machinekit/machinekit/issues/689#issuecomment-169641909 https://github.com/machinekit/machinekit/issues/689#issuecomment-171910982 https://github.com/machinekit/machinekit/issues/689#issuecomment-171911190

ArcEye commented 6 years ago

Comment by mhaberler Fri Jan 15 10:17:07 2016

I alluded to this a bit in https://github.com/machinekit/machinekit/issues/689#issuecomment-171916996

actually we could reuse https://github.com/mhaberler/machinekit/tree/jplanner-queued to do the homing and once done defer to the playout interpolator, switching jplan+homing off

what is needed in jplan is switch/trigger support and stopping in minimum distance, plus switch trigger position recording

I would love to have these be different components and glue them together at the pin level

a way to do this could be to have both jplan and the interpolator feed into HAL_IO pins for pos/vel if active, and just set the internal position from reading those pins whatever the other party does when inactive; this is what motion does to synchronize planners, it just does not do it at the HAL pin level but in internal joint position state which is why it has to be in a single comp

ArcEye commented 6 years ago

Comment by luminize Fri Jan 15 10:37:02 2016

I too would love components to only be glued at pin level. That's real modularity

Don't we need a third component: joint_state ? which is only a minimal state engine with states like UNHOMED, HOMING, HOMED, ACTIVE? together with the jplan and interpolator makes for a minimal setup.

how do we go about "smart" drivers, like canopen/ethercat, where homing can be done by the driver. Should we use an input pin "externally-homed" that we can trigger thru python?

ArcEye commented 6 years ago

Comment by mhaberler Fri Jan 15 11:04:51 2016

well I would say if there is state it should be owned by the comp which operates on that particular state, suggesting if there is a homing component which can home a/any joint then that comp would be the one to own that pins

in the case of IO pins it does not really matter if they are exported by several comps since they need to be wired anyway

jplan needs to be involved into a homing-capable comp, then it would be natural to have those pins in it

in the smart drive scenario, I think it would be the driver comp which can tell if joints are homed, so I guess it would export those pins

a separate state comp without any operations on that state does not make sense; but if there are different ones which can be linked together as needed that's just fine

ArcEye commented 6 years ago

Comment by mhaberler Fri Jan 15 11:25:25 2016

actually wiring all of pos, vel, acc pins together as HAL_IO pins has one obvious advantage: switching can happen on the fly, this also relates to the interpolator discussion in #689

Assume for instance (this is pretty made up and ambitious, but anyway):

jplan does a move. That drives pos, vel, acc pins.
during the move a switch happens from jplan to the interpolator, which already has trajectory segments waiting in the queue
jplan gets disabled, leaves pins alone mid-flight
the last jplan-generated pos, vel and acc values become the starting value of the interpolator, and the first segment directly blends into the jplan-generated move; without jerk since there is acc continuity (if quintic)
in theory the switch back to the joint planner should work fine as well; if it is disabled the move comes to a stop; if enabled, the move continues as driven by the joint planner (with jerk, but such is that planner)

the more I think of it this whole flush-the-motion-queue-on-planner-switch business is another red herring. Coming to a stop before switching probably is a good idea, but having a design that permits on the fly switching certainly is less restrictive

ArcEye commented 6 years ago

Comment by luminize Fri Jan 15 12:23:14 2016

in fact, there's nothing that limits us to just 2 queues (homing motion, segments). a bit far fetched maybe... I can imagine multiple queues that are waiting. Like a queue that tracks a conveyor belt, a queue that runs an off line planned path, another queue for a tool change, a queue for a jog while pause.

ArcEye commented 6 years ago

Comment by mhaberler Fri Jan 15 12:52:40 2016

no, but then returns are diminishing the right thing to do is to add a selector pin which will index one of a set of ringbuffers (as many as are defined/attached) and fail if this one does not exist

as you say that whole thing was already exercised in motion with the jog-while-paused alternate queue so no surprises

ArcEye commented 6 years ago

Comment by jpmoniz Wed Jan 20 00:12:29 2016

I would also include in the homing allowances for absolute feedback. Perhaps the component could operate in either mode. a reference offset would have to be stored in HAL and retrievable on system startup. The act of homing an absolute joint would just simply redefine the stored offset.

ArcEye commented 6 years ago

Comment by jpmoniz Wed Jan 20 02:45:19 2016

@luminize @mhaberler Looking at the jplan code Does it not make sense to add simple homing functions within the comp. I've been looking at the homing code in motion and at first glance it looks somewhat straight forward to add home to a switch functionality to jplan. Do you really need a queue for homing? I guess the queue could be useful for more complex homing other than move to switch and call it done. Id be willing to take this little exercise on. My thought would be that the homing command is some what fixed (based on joint limits + a little extra) no different then it is in motion right now.

Sequencing of multiple joints could be handled thru python drive each comp instance as required and also report state if needed.

My thoughts

"joint min max limit pins" "is referenced flag"- If mode is absolute always true. If incremental only true after homing "Home enable pin"- Executes homing if incremental. if absolute just calculates new offset "Home sw input pin" "Home Dir pin" -change home command direction "Home Vel Pin" "Home Acc Pin" an "Absolute pin" - 0 = incremental 1 = Absolute an "Absolute offset pin" for info perhaps? a "home fault pin" returns fault code

Thoughts or am I way off?

ArcEye commented 6 years ago

Comment by mhaberler Wed Jan 20 17:55:12 2016

@jpmoniz - I diverted a bit to do an RT interpolator first, integrating with homing is next

wrt to what goes in the joint planner comp for homing and other functions: it seems to me stuff is glued together in motion at a way too low level and really there is no need for separate planners as they did in motion

have a look at the interpolator branch, maybe build and play with it, here's a halscope shot with doing a quintic spline interpolation:

for homing we need a trigger pin so the switch edge can be latched at rising or falling edge the rest really can be done in the Python feeder code, and even the homing run could travel through the interpolator - I want to keep the RT code as minimal as possible

ArcEye commented 6 years ago

Comment by luminize Wed Jan 20 17:59:48 2016

oh wauw!

ArcEye commented 6 years ago

Comment by jpmoniz Thu Jan 21 03:22:51 2016

@mhaberler

I built your interpolator branch and was able to run your examples.Both the jplan and interpolator. So if I understand you right one-shot rising/or falling within jplan. This would capture the current position of the joint and then apply offset elsewhere or within jplan? I agree that homing sequencing / control would be better as a python comp. What about joint limit switches where do you see those interfacing?

Ah taking a second look. I was thinking command and feedback was happening in jplan. I see that is not the case. As the name implies it just plans. So offsetting the known home from the feedback will happen elsewhere. Where would you see the rationalization of command vs feedback higher up the chain or lower?

ArcEye commented 6 years ago

Comment by cdsteinkuehler Thu Jan 21 14:28:58 2016

On 1/20/2016 11:55 AM, Michael Haberler wrote:

for homing we need a trigger pin so the switch edge can be latched at rising or falling edge the rest really can be done in the Python feeder code, and even the homing run could travel through the interpolator - I want to keep the RT code as minimal as possible

I was thinking of writing a simple joint-control component that would sit between the position stream generated by the interpolator and the motor drive. This component would limit maximum vel/acc, and would implement some (or all?) of the homing schemes supported by the CNC stack homing (which isn't really all that complicated). There would be some state output (homing/homed) and a trigger to initiate the homing sequence. Homing sequencing would be handled by some other component (likely python user-mode code).

This isn't as grand as what you're looking at with multiple motion queues, but it seems like it's simple and could be fairly handy.

I'll hold off if you think this would be too confusing, or if you think a multi-source motion homing scheme is a pretty quick and easy project (it seems hard to me, but I'm not a Python guru).

Charles Steinkuehler charles@steinkuehler.net

ArcEye commented 6 years ago

Comment by mhaberler Thu Jan 21 17:24:45 2016

combining the basic homing support with the interpolator makes sense; I do not think a separate comp is needed - pos/vel/acc clamping could also happen on the driving side (python), or in the interpolator itself (probably safer)

what we'd need in the interpolator is some minimal per-joint state, like a pin reflecting mode: {UNHOMED, SEARCHING_FOR_POSITIVE_EDGE, SEARCHING_FOR_NEGATIVE_EDGE, STOPPING, STOPPED, HOMED, TIMEOUT}

a joint starts out in UNHOMED unless explicitily set otherwise, which would make it ignore motions until HOMED
a home search would be initiated by the userland side by setting the state to SEARCH_FOR_whatever_edge and queuing a move
in SEARCH_FOR_xx, the home switch closing or opening would record the current position in a pin, then a stopping move with maximum accel (really decel) would be executed (queued internally) (state STOPPING)
when stopping move complete -> STOPPED
when the switch did not close at end of move/after X secs -> TIMEOUT
now the userland side can trigger on state STOPPED, record the home offset, and switch the joint to HOMED
any segment pushed down now has the homing offset applied as recorded

for persistent homing offsets, things have become simpler - since we do not have to bother about kernel modules any more, the instantiate and delete methods in the RT comp would be fine to just write to/read from a plain file (if we had a parameter service worth its powder we'd use that)

technically rtapi_app_main/rtapi_app_exit/instantiate/delete are plain userland code, executed by a non-RT thread so everything's fair game here

ArcEye commented 6 years ago

Comment by mhaberler Sat Jan 30 17:54:00 2016

thought through this and I did not like it - too much state and assumptions in the RT comp, I just want some low level primitives there and the rest to be controlled by commands over the ringbuffer

my current minimum in-RT support to do homing and index search is a primitive which would - if so commanded by userland:

if the switch pin changes state, record the position in a pin
end the current segment (essentially time-warp forward to end of segment).

the command sequence would look like so (assuming interpolator behavior):

queue a very long move with max accel to desired search velocity
that move command gets tacked on a bit telling to watch for the switch pin, and which edge (pos/neg)
also queue a move with max decel to vel 0, that is our stop move (length easily computed if you have search vel and max decel permitted - v_v/2_a)

what happens in the RT comp is:

move starts, while monitoring for desired switch edge if told by command bit
if edge detected, position is recorded and the current segment is ended
which causes the playout of the next segment, which is the stop move

with that primitive AFAICT all homing methods in the motion homing FSM can be implemented, but rather as Python code in userland.

index homing is just a tad different: the switch edge essentially arms the index edge search (because the index transitions on every revolution whereas the homing switch transitions once only), and position capture would happen only at the index transition

not everybody needs index homing, or homing at all, so I would make that a instantiation parameter: none, switch1..switchN, switch1..N+index - the switch and index pins would be created only if told to do so

the search move hence is a simple line segment with these optional attributes:

search for switch transition plus transition polarity (pos/neg)
search for index transition plus transition polarity (pos/neg)

I think that is all what is needed for homing support within RT, the rest becomes Python

ArcEye commented 6 years ago

Comment by luminize Sat Jan 30 20:32:38 2016

I have the same view. Homing means record at which position the flank happens. Then add/subtract that "offset" to the needed position command values. No need to set the position, just record the mismatch.

There needs to be thought about not only the flank, but also a startup with the switch already pressed. Start with switch already pressed means change in direction and find index/flank)

Possible inputs:

Limit switch (Pos/neg) (monitoring, not urgent IMO) Reference switch Encoder Index

I did measurements in the past about repeatability of normal (few euro) end/limit switches, and it's a sad story. Normally with trivial kinematics not a problem. But non-trivial kinematics need that repeatable setup. Hence the need for (I have not measured, but think it has way better repeatability ) the accurate encoder index pulse after a "rough" reference switch.

ArcEye commented 6 years ago

Comment by cdsteinkuehler Sat Jan 30 21:10:21 2016

@mhaberler So this sounds a lot to me like a straight-forward "probe" move. I like the idea of implementing minimal real-time support and leveraging that via user-land (Python) code for homing, but I have some questions:

What switch(es) are supported? You list home and index, but what about pos/neg limit switches? Are any others needed? What support do limit switches need on the real-time side?

What is doing the motion planning? If it's the user-land code feeding smooth paths into real-time, what happens when the switch trips and a maximum speed stop is required? Where is that motion planned?

Does the probe/homing sequence go through the same motion ring-buffer to real-time as the normal operating commands?

Where are the joint offset calculations applied...in real-time, user-land, or a mix of both?

I think all forms of homing can be handled by a single sufficiently flexible "probe" move, where the move can specify the switch(es) to trigger on and the polarity (hi, lo, rising, falling, any-edge). Index homing is just a slightly more complex case of standard two-step homing, simply looking for the index pulse rather than a switch closure.

ArcEye commented 6 years ago

Comment by jpmoniz Sat Jan 30 21:23:32 2016

I played around using that same basic concept this week using the jplan branch. I just wrote a simple comp to record the value of a float input pin based on bit trigger. the comp also calculated the offset at time of recording. That offset could then be fed back into the feedback path through the homing SM python code.

This does make me wonder if there is validity to having a joint structure component in HAL? Not with the intention of putting everything and the kitchen sink in the structure but more to consolidate information related to a joints operation/function.

ArcEye commented 6 years ago

Comment by luminize Sat Jan 30 22:28:52 2016

Taking one step back. Can't we simplify homing by forgetting queues, and just give the joint stepgen only a velocity, and record the position like above. Then just use this offset (startup minus recorded position) in the motion position pipeline. Like what @jpmoniz did with the J0 etc offsets in the libeardeltakins

ArcEye commented 6 years ago

Comment by jpmoniz Sat Jan 30 22:44:29 2016

I think @mhaberler approach makes sense in what would be required in an RT component. Then it becomes how to apply the offset and when. IMO the offset should be applied to the feedback.Command should be ingnorant of any offset. This is why I think the need for a joint comp exists with a minimal structure for now. The components would just provide the means where user land code can manage when/state

ArcEye commented 6 years ago

Comment by mhaberler Sat Jan 30 23:13:37 2016

@jpmoniz the problem with my approach is that the joints operate in parallel in the current structure (all joint segments end/start at the same time) but homing is a per-joint activity with per-joint timelines, and right now it is not that natural to switch between those two, even if reusing the interpolator as a simple tp for homing purposes looks like an easy proposition

there are several ways to handle the issue:

a single queue where the structs passed support NULL values (meaning do not touch this joint) and multiplexing of activity happens in the driving side of this single 'all joints' queue
per-joint queues: makes it easy to handle per-joint individual ops+state, but synchronizing the queue's timeline might become an issue
switchable queues (like the jog-while-paused branch does) - maybe a single one for all joints for coord playout, and a per-joint queue for individual commands
a combination of 'either by pin or by queue' scheme which might be simpler for homing and other simple probe-like tasks

I think (1) is the most flexible scheme because 2, 3, 4 can be layered on top in a separate comp if needed (the missing NULL value in EmcPose fields is one bummer of a design defect in EMC)

I think we need to step back a bit and do proper requirements first - @cdsteinkuehler alluded to that already

(what I originally had in mind was to just explore that joint planner @jpmoniz and I discovered, then the interpolator and that turned out to be a good demo for a ROS trajectory playout device; to ready it for production use we need a bit more rigor.. so let's do that exercise first: what application and features do we address)

ArcEye commented 6 years ago

Comment by mhaberler Sat Jan 30 23:42:38 2016

let's do requirements

scope: a component which supports, or a set of components which in concert supports:

playout of a segment queue (lines only, all segments end at the same time), with the primary purpose of supporting a ROS stack
individual per-joint operations: -- jogging moves -- homing to a switch -- optionally homing followed by an index search -- immediate homing aka redefine/update home offset (Absolute no motion required) -- limit switches (shared and unshared)
observers (pins for now): -- a joint has stopped
startup and shutdown - which will entail recording current offsets and loading on startup if relative or no encoders are used
defined behavior on abort/E-stop
defined behavior of joints on underrun of the input queue (does the joint abort if a motion is in progress and no next segment is at hand?)

(edit as you see fit)

ArcEye commented 6 years ago

Comment by mhaberler Sat Jan 30 23:44:47 2016

on the above:

there are issues with coordinate system, units and offsets which go back a long time and we should avoid falling into the same trap again while redoing parts of motion in a more reusable fashion:

EMC/lcnc chose to do everything in user coordinates and units and applies offsets way too early (in canon). Basically the whole motion subsystem is coerced to view the machine through the current units/offsets whatever the machine coordinates are for "ease of use"

The legacy code even fiddles the machine coordinates during homing (it resets the machine coord to zero on home/index) which is the reason why motion is a complete bummer when working with absolute encoders.

This is probably the wrong approach - the machine coords may never be touched, just offsetted and scaled.

I think the servo loop and the interpolator need to work in machine coordinates and units to support absolute encoders; transforming back machine coords back to user offsets/units can be handled by a separate comp.

I am not sure joint state can simply be aggregated into a comp - one problem being: you can make a comp export pins BUT ALL the processing on the pins must reside in this comp

ArcEye commented 6 years ago

Comment by jpmoniz Sun Jan 31 04:29:59 2016

@mhaberler Ok I was thinking interfacing with the CNC stack was not in scope and this was more creating a base layer of components supporting the use of ROS. Making more sense now!

ArcEye commented 6 years ago

Comment by mhaberler Sun Jan 31 09:09:19 2016

@jpmoniz I do not exclude this could eventually slip under the task+interpreter, but I think the first goal here is to support ROS trajectory playout - or some other form of trajectory generation, maybe just scripts

will state that in the requirements

it's a pity motion is almost the only game when it comes to trajectory generation and playout - does way too much in a too arcane way

ArcEye commented 6 years ago

Comment by luminize Sun Jan 31 10:04:59 2016

@mhaberler @cdsteinkuehler @jpmoniz I think 85% of functionality is already in there. In your opinions, what do we really need outside the already available components? (without discussion what's in the RT domain and what not, because I think it's not a real issue now)

there are a variety of mux component, so we can select between inputs.
we can put a stepgen in velocity mode (search switch velocity, search index velocity)
so we have reverse behaviour on a (changing) pin value
We have a basic interpolator.
We have a basic planner
We have ringbuffers.
We have the oneshot component

The only thing i can think of is that we lack a way of recording the offset when the "home signal (digital input)" is triggered.

Why not prototype behaviour first with existing HAL components and add small specialised components for specific behaviour? We can always get smarter later.

Is the above worth trying out or you think I'm plain wrong?

ArcEye commented 6 years ago

Comment by mhaberler Sun Jan 31 10:06:18 2016

on (6) defined behavior of joints on underrun of the input queue:

my initial suggestion relied on queued segments to stop motion after a switch triggers, so I get around the requirement to execute a stopping decel segment internally

while this is minimal in terms of code within the RTcomp, it might not be a great idea in terms of safety

I think the playout comp does need to support a decel to a stop internally similar to the tp's tpAbort() method - to stop on switches, but also on E-stop (disable pin)

that in turn requires som internal per-joint state, my current minimum being:

enum joint_state {
    JS_DISABLED,
    JS_UNHOMED,
    JS_HOME_SEARCH,
    JS_INDEX_SEARCH,
    JS_ENABLED,
};

also just noting motion really does not know when a segment queue ends - which is the underrun scenario I alluded to - what the tp does is to stop a somewhat brighter method would be to tag a continuous sequence of segments with a begin_traj/end_traj command so the comp can determine if a sequence ended, or the driving layer just went away or ran too slow

the scenario I am concerned about is: assume motion in progress, end velocity of current segment > zero, so joint moving, then there is no followup segment in the ringbuffer for some reason

that probably needs a decel-to-stop move internally as well, one cant just continue going at the current rate if somebody tripped over the cable, which also suggest an internal decel-to-stop capability (really the equivalent of tpAbort() without the tp API being present)

ArcEye commented 6 years ago

Comment by mhaberler Sun Jan 31 10:45:27 2016

@luminize

there are a variety of mux component, so we can select between inputs. yeah but that does switch pins, not queued inputs, so not a help; but as I said - doing a queue mux component is possible
we can put a stepgen in velocity mode (search switch velocity, search index velocity) means moving the trigger+stopping issue from the joint planner/interpolator comp one step down the pipeline (where it does not belong), PLUS you cannot assume there will be only stepgens - you can only assume the interface to hardware is a commanded position / commanded velocity pin, not some particular hw

this is about a sequence of moves in either scenario (playout or homing), not moving once somewhere, which is why pins are not the right vehicle - a queue is because it records a sequence

ArcEye commented 6 years ago

Comment by mhaberler Sun Jan 31 11:11:16 2016

taking the multiple queue idea a step further:

it seems to me it would help if we had a generic ring multiplexer component:

it would attach to a 2..N input ringbuffers, and an output ringbuffer
it works similiar like a pin multiplexer but on the input ringbuffers, not input pins
it would have a selector pin which tells the mux comp which input ring messages to pass through to the output ring

assume we have that and put that in front of the interpolator we'd have say three input rings the first one is fed with the segment queue the second one is fed from the homing Python code the third one could be one which takes jog commands from some input

now the selector pin can be driven by some comp which manages the state of the setup, like "running", "homing", "jogging"

the inverse function would be a ring demultiplexer - an input ring being demuxed to any number of output rings

since this stuff is queued it would not be necessarily have to be run from an RT thread but we have the 'vanilla thread' posix option on "RT threads" already

these comps would allow us to assemble pipelines of different comps which pass messages, not pin values (really an old idea Bas and me already chewed upon a long time back)

ArcEye commented 6 years ago

Comment by luminize Sun Jan 31 11:53:46 2016

The above: ok, fair enough. I think a lot of thoughts are the same and differ on minimal points. But I'm stubborn :)

Homing IMO is not only about queued moves (which is one solution/implementation). It's about getting the machine from a unknown state to a known one. We should be able to rewire the transition logic without changing homing behaviour. Take block homing for example. We should be able to use a motor current threshold, or any other combination of Hal component building logic for a state transition, without rewriting the component.

Then what's also missing next to a queue mux component?

A behavioural/monitoring component on top, switching/driving different components? Like:

if unknown (startup stat) then fill queue 1 and start moving and listen to inputs
If I see the flank of a switch I do a next move or record the startup error (depending on homing behaviour)
If I know the position wrt the real world, then I'm homed and switch to another queue of a mux queue component

I might misunderstand or over simplify but I think strength is in cutting up in as many specialized building blocks, and configure them together with the correct plumbing (configuring instead of programming).

How would your points be implemented as external "logic" component, only triggering transitions in the joint component?

The joint component would only make sure for example that there's no jogging while doing queued moves, or homing or similar.

ArcEye commented 6 years ago

Comment by luminize Sun Jan 31 12:01:38 2016

@mhaberler I just missed your reply . I'm chewing on the ring multiplexer now.

ArcEye commented 6 years ago

@mhaberler commented on 31 Jan 2016

yes, I envisage a severely butcher up design of several comps, with something like a management comp/script glueing things together

I think @jpmoniz ' idea of a limit/switch detection comp is one more thing to parcel out; it would be staged behind the interpolator, monitor soft limits, limit switches, home and index pins

it would also abort a move by a pin between the interpolator and the limit component

the management comp would monitor the pieces and state, and switch the input mux as needed

I need to nap over it, will draw a setup image tomorrow

@mhaberler commented on 18 Mar 2016

https://github.com/mhaberler/machinekit/commits/interpolator now has a basic ring multiplexer component

it enables jplan to take commands from one of several input rings; mhaberler@c538120 shows a usage example.

machinekit / machinekit-cnc

Need for per joint homing behaviour #27