Program Counter / Pointer to the actual mission executed

MattiaDeiRossi

Goodmorning all,

I would like to keep track of all the movement execution with a variable or a pointer between consecutives movement. What's the best way to do it?

Using the Python API is quite ambiguos because when a new "MotionProgram" is created a "play" method will trigger the entire sequence of movements.

I don't know if you have some built-in way to use this feature

Coen_Smeets

Hello,

If I understand the question correct, then there are multiple parameters in the tree that keeps track of the current command of the motion-player:

root/MotionInterpreter/currentCommandId   id of the command executing
                                          right now (0 when none)
root/MotionInterpreter/previousCommandId  id of the last command that
                                          finished
root/MotionInterpreter/currentProgramId   id of the running program
root/MotionInterpreter/commandStatus      array [current_id, status, error]

By remembering the order in which commands were added (a simple dict of id -> label), the client can correlate the live currentCommandId with its own description of each movement -- without splitting the program into separate single-move programs.

Caveats:

Polling can miss very short commands: RobotCommand.play() sleeps for
wait_time (default 1 s) before returning, so a command that completes
within that window is only visible through previousCommandId. To catch
every transition, lower wait_time and/or subscribe to currentCommandId
on the subscribe connection instead of polling.
With waypoint smoothing/blending the handover between two consecutive
move commands is not a sharp point; the id flip is approximate there.
currentCommandId drops to 0 between commands and after stop/reset; use
previousCommandId to know which command just finished.

Hopefully, this answers the question.

Coen

MattiaDeiRossi

Goodmorning,

I focused on root/MotionInterpreter/currentCommandId and I notice that this counter grows just a little bit
from 0 to 2 even if my motionProgram contains a lot of separate instructions:

    motion_pr_11 = MotionProgram(req, motorcortex_types)
    motion_pr_11.addMoveJ([wp1], 0.2, 0.1)
    motion_pr_11.addMoveL([wp2], 0.2, 0.1)
    motion_pr_11.addMoveJ([wp3], 0.2, 0.1)
    motion_pr_11.addMoveJ([wp4], 0.2, 0.1)

    motion_pr_11.addMoveL([wp5], 0.2, 0.1)
    motion_pr_11.addMoveJ([wp6], 0.2, 0.1)

That's mean that it seems that I cannot use this variable to track the all the movement.... I don't know if this behaviour is correct or not.

In any case, this "counter" should be very useful for us to correctly manage all the "recoveries" of the robotic arm, so as to be sure to execute correct trajectories excluding possible inconveniences.

Coen_Smeets

Hi Mattia,

I tested this on a controller here with exactly your program shape (6 commands, alternating addMoveJ/addMoveL, one waypoint each) and currentCommandId does step through every id, 1..6:

t= 0.121  current=1 previous=0  state=RUN
t= 0.140  current=2 previous=1
t= 1.271  current=3 previous=2
t= 2.791  current=4 previous=3
t= 4.271  current=5 previous=4
t= 6.751  current=6 previous=5
t= 7.971  current=0 previous=6  state=DONE

So the counter is fine — what you are hitting is a sampling problem, and play() is the main cause:

RobotCommand.play() sleeps wait_time (default 1.0 s) before it returns. That whole program above finished in 8 s, so by the time play() hands control back you are already on command 2 or 3. Call robot.play(0.0) and start observing before/immediately after it.
The first transition took 20 ms. If the robot is already at your first waypoint (which it is, after moveToStart), command 1 is a zero-length move and completes in a single cycle. No polling loop will catch that — only previousCommandId will show it happened.
Polling on the request connection is too coarse. Subscribe instead, so you get a sample every control cycle:

sub_ids = sub.subscribe([
    "root/MotionInterpreter/currentCommandId",
    "root/MotionInterpreter/previousCommandId",
    "root/MotionInterpreter/actualStateOut"], "ids", 1)
sub_ids.get()
...
while True:
    vals = sub_ids.read()
    if vals:
        cur, prev, state = [v.value[0] for v in vals]

Two more things that bit me while testing:

commandStatus[0] leads currentCommandId — it showed 2 while currentCommandId was still 1, because it reports the command being prepared, not the one executing. Use currentCommandId for "now" and previousCommandId for "just finished".
With waypoint smoothing/blending the handover between two moves is not a sharp point, so the id flip there is approximate by nature.

For your recovery use case I would drive it off previousCommandId rather than currentCommandId: it is monotone within a program run and never drops to 0 mid-program, so "last command that definitely completed" is exactly the resume point you want.

Coen