enable_alter_motion()

Definition

enable_alter_motion(n,mode,ref,limit_dPOS,limit_dPOS_per)

Features

enable_alter_motion() and alter_motion() functions enable altering of the motion trajectory.

This function sets the configurations for altering function and allows the input quantity of alter_motion() to be applied to motion trajectory. The unit cycle time of generating altered motion is 10msec. Cycle time(n*10msec) can be adjusted using the input parameter n.

This function provides two modes: Accumulation mode and Increment mode. Input quantity of alter_motion() can be applied to motion trajectory in two ways: as an accumulated value or as an incremental value.

• In Accumulation mode, the input quantity means the absolute altering amount from current motion trajectory.

• In Increment mode, the input quantity means the incremental value from the previous absolute altering amount.

The reference coordinate can be changed through input parameter ref. Limitations for accumulation amount and increment amount can be set using input parameters limit_dPOS (accumulated limit) and limit_dPOS_per(incremental input limit during one cycle). The actual altering amount is constrained by these limits.

Parameters

Conditions of use

How it works

For each alteration the controller plans a rest-to-rest trajectory over the path generation period T_n = n × 10ms. The controller accepts only one alteration per T_n. The acceptance window opens on the control cycle in which a new trajectory starts and closes the moment the first alteration to arrive after that is taken; any value arriving later within the same T_n is neither stored nor reported, but discarded without an alarm. In other words the controller does not overwrite with the newest value — it is first come, first served, so when two alterations arrive within one T_n it is the later one, the fresher alteration, that is discarded. A trajectory in progress is never preempted by a new alteration; the next one takes over during the deceleration phase of the current one and is summed with it. So when one alteration arrives on time in every T_n, the motion is connected without a dwell; when it does not, the trajectory finishes first and waits at a standstill for the next alteration.

In other words, n sets how long the controller spends on one alteration, so it must be chosen to match the period at which the DRL can actually deliver alterations. Proceed as follows.

① First determine the period at which alterations can be delivered

The period at which alteration values are produced (sensor, vision frame, upper-level control period) is often already fixed by external factors, but that value is not the period at which they are delivered to the controller. The interval at which alter_motion() is actually called — called the supply period in this document — is that value plus the time the DRL thread needs for one turn: the alteration computation, the cost of the alter_motion() call, contention with other threads, and jitter. In a measured case a thread intended to run at 30ms with wait(0.03) had a supply period of 36ms alone and 73ms while a monitoring thread was running.

Therefore do not assume the supply period — measure it by running the motion and thread configuration you will actually use. The condition must hold in every period, so use the maximum rather than the average. Turn the execution line display function OFF before measuring so that the supply period does not move with the amount of code (section 1 below). The measurement procedure is in section 2.

② Choose n to match that period

• Lower bound — mandatory. n must be at least the maximum supply period ÷ 10ms rounded up. This applies to both modes.

• Accumulation mode (DR_DPOS) — above the lower bound a larger n is quieter (at 1.5 × the supply period the vibration was 1.6 times the unaltered motion, at 2.5 × it was 1.3 times), so use the largest value your delay budget allows. The only cost is the applied delay (about 2 × T_n); there is no loss of accuracy.

• Incremental mode (DR_DVEL) — keep n as close to the lower bound as possible. Where the period fluctuates, increments can be dropped intermittently, so build a closed loop in which a sensor keeps measuring the error remaining after the alteration is applied, so that what was dropped is corrected on the next measurement (section 4). If a closed loop is impractical, use the accumulation mode.

• The valid range of n is 1 to 254 (10 to 2540ms). Outside the range a warning is raised and the path alteration function is not enabled.

measured supply period  avg 72ms / max 74ms

  lower bound : n x 10ms >= 74ms  ->  n >= 8

  DR_DPOS : delay budget 250ms  ->  n = 11 (110ms, approx 220ms of delay)
            with a larger budget, n = 18 (180ms) is quieter
  DR_DVEL : n = 8 (80ms). keep it at the lower bound and use a closed loop
            so that dropped increments are corrected

③ What goes wrong below the lower bound

In every period where the supply is late the trajectory finishes first and the alteration waits at a standstill. The displacement itself is preserved, but less time is left to move it, so the peak velocity grows by 1/k and the peak acceleration by 1/k² (k = T_n ÷ supply period), and that is what becomes vibration and noise. The most common cause is skipping the measurement — where the wait() value was assumed to be the supply period, the vibration was 10 times that of the unaltered motion.

1. Turn OFF the execution line display function

Turning the execution line display function OFF is what removes the time delay that the other DRL lines in the same thread add while the alteration position is being delivered. While the function is ON, every line of the DRL sends its execution line to the TP, and in an alteration thread that cost accumulates in the time taken to compute the alteration position and deliver it through alter_motion(). The supply period is then determined by how many other DRL lines that thread executes rather than by the wait() value, so changing a few lines of the alteration logic changes the period and invalidates the supply period established in ①.

drl_report_line(0)      # makes the supply period independent of code size

In a measured case, 12 lines of alteration waveform computation accounted for 11.2ms; with the execution line display function OFF that difference dropped to 1.5ms or less. Once it is OFF, the supply period becomes the wait() value plus a nearly constant remainder, so it can be set through wait().

• In the section where the function is turned OFF, execution time display by line, variable monitoring, system variable update, and Step by Step / Brake Point in Debug mode do not operate. Keep it ON during development and turn it OFF for operation.

• Turning it ON or OFF changes the supply period. If you change it, redo the measurement and the choice of n below.

• With the function OFF, exceptions are caught by except Exception (with it ON, by except KeyboardInterrupt).

2. Measure the supply period

Measure under these conditions.

• If the robot is stationary, the motion generation load is absent and the value comes out short. Measure while the base motion is running.

• Skipping the alteration computation also shortens it. Set only the alteration gain to 0 and keep the computation and the call running.

• If you add or remove a thread, or change the alteration logic, measure again and re-choose n.

• Calling get_current_posx() or get_desired_posx() inside the loop causes a controller round trip and lengthens the period. Use the get_desired_posj() family where joint values will do.

3. Do not artificially shorten the supply period

Shortening the supply period reduces the position tracking error, but the same displacement must then be produced in less time, so the acceleration ripple grows. If noise or vibration is the problem, shortening the period is counterproductive.

Keep the period steady rather than short. Large jitter crosses the lower bound in every period.

4. Use the incremental mode (DR_DVEL) only in a closed loop

If an external sensor keeps measuring the error remaining after the alteration has been applied, the error does not shrink by the increments that were dropped, so the next measurement retries them automatically. Conversely, when you send increments accumulated from time or an encoder (for example v × dt), this is an open loop and the dropped increments are lost permanently. No alarm is raised; the alteration simply falls short.

In an open loop, keep the running total in the DRL and send it in the accumulated mode.

# Sending an open-loop incremental source in the accumulated mode
var_total = var_total + var_delta
alter_motion(posx(0, var_total, 0, 0, 0, 0))    # DR_DPOS

Unless you specifically need increments in the tool coordinate system, the accumulated mode is recommended.

Exception

Example

# ---------------------------------------------------------------
# Real-time path alteration
#   Example 1  accumulated mode (DR_DPOS) - when the absolute offset is known
#   Example 2  incremental mode (DR_DVEL) - when a sensor measures the remaining error
# ---------------------------------------------------------------
drl_report_line(0)    # makes the supply period independent of code size

DT      = 0.05        # base supply period [sec]. the actual period is larger
AMP     = 5.0         # example 1 alteration amplitude [mm]
FREQ    = 0.25        # example 1 alteration frequency [Hz]
GAIN    = 0.5         # example 2 correction gain (0 < GAIN <= 1)
LAT_MAX = 0.25        # example 1 allowed delay [sec] = about 2 x n x 10ms
LIM_POS = [50, 90]    # limit_dPOS     : total alteration limit [mm, deg]
LIM_INC = [10, 10]    # limit_dPOS_per : per-period increment limit [mm, deg]

g_mode  = 0           # 0 = accumulated / 1 = incremental
g_gain  = 0.0         # alteration gain. 0 applies no alteration
g_t0    = 0.0
g_calls = 0
g_prev  = 0.0
g_max   = 0.0         # maximum measured supply period [sec]

def read_path_error():
    # Replace this so it returns the current remaining path error [mm]
    # measured by an external sensor. It must be the error that remains
    # after the alteration has been applied.
    return 0.0

# thread_run(loop=True) calls the function again each time it returns, so do
# not put a while loop inside. With a while loop, thread_stop has no effect.
def alter_thread():
    global g_calls, g_prev, g_max
    if g_mode == 0:
        var_y = g_gain * AMP * sin(6.28318 * FREQ * (time.time() - g_t0))
        alter_motion(posx(0, var_y, 0, 0, 0, 0))       # the offset that should apply now
    else:
        var_e = g_gain * GAIN * read_path_error()
        alter_motion(posx(0, var_e, 0, 0, 0, 0))       # add a fraction of the remaining error
    # Measure the call interval every period and keep the maximum.
    var_now = time.time()
    if g_prev > 0.0:
        if var_now - g_prev > g_max:
            g_max = var_now - g_prev
    g_prev  = var_now
    g_calls = g_calls + 1
    wait(DT)

# End poses of the base motion - use the commanded pose (get_desired_posx),
# not the measured one. The measured pose contains tracking residual, which
# accumulates as error if fed back as a target.
movej(posj(30, 45, 45, 0, 90, 0), vel=60, acc=60)
X1 = get_desired_posx(DR_BASE)
movej(posj(-30, 45, 45, 0, 90, 0), vel=60, acc=60)
X2 = get_desired_posx(DR_BASE)

# --- Measure the supply period --------------------------------
# Run once with the motion and thread configuration you will actually use.
# g_gain = 0.0 makes the alteration zero while the computation and the call
# still run, so the robot path is unchanged and the load matches real use.
movel(X1, vel=30, acc=60)
mwait(0)
enable_alter_motion(n=20, mode=DR_DPOS, ref=DR_BASE,
                    limit_dPOS=LIM_POS, limit_dPOS_per=LIM_INC)
g_mode  = 0
g_gain  = 0.0
g_calls = 0
g_prev  = 0.0
g_max   = 0.0
g_t0    = time.time()
th_id   = thread_run(alter_thread, loop=True)

movel(X2, vel=30, acc=60)

thread_stop(th_id)
disable_alter_motion()

var_supply = DT                                # fallback if the measurement fails
if g_calls > 5:
    var_supply = g_max                         # maximum supply period [sec] - basis for n
tp_log("supply max = {0} ms".format(int(var_supply * 100000) / 100.0))

# Lower bound : maximum supply period / 10ms, rounded up
N_MIN = int(var_supply * 100)
if var_supply * 100 > N_MIN:
    N_MIN = N_MIN + 1
if N_MIN < 1:
    N_MIN = 1

# --- Example 1. accumulated mode (DR_DPOS) --------------------
# Every transfer is a complete offset. If the controller does not accept one,
# the next transfer restores it, so nothing is lost.
# Take the largest n the delay budget allows.
N_DPOS = int(LAT_MAX * 100 / 2)
if N_DPOS < N_MIN:
    N_DPOS = N_MIN                             # the lower bound wins

movel(X1, vel=30, acc=60)
mwait(0)                                       # start the alteration from standstill
enable_alter_motion(n=N_DPOS, mode=DR_DPOS, ref=DR_BASE,
                    limit_dPOS=LIM_POS, limit_dPOS_per=LIM_INC)
g_mode = 0
g_gain = 1.0
g_t0   = time.time()
th_id  = thread_run(alter_thread, loop=True)

movel(X2, vel=30, acc=60)                      # base motion with the alteration

# Shutdown order : stop the alteration -> thread_stop -> disable_alter_motion
# If the thread is still alive after disabling, its alter_motion() calls fail.
g_gain = 0.0
thread_stop(th_id)
disable_alter_motion()

# --- Example 2. incremental mode (DR_DVEL) --------------------
# Use only in a closed loop where a sensor keeps measuring the remaining error.
# Increments the controller does not accept never come back, but in a closed
# loop the next measurement retries them automatically.
# Keep n at the lower bound. Increments dropped intermittently are
# corrected by the closed loop on the next measurement.
# If read_path_error() is not a closed loop, use example 1 instead.
N_DVEL = N_MIN

movel(X1, vel=30, acc=60)
mwait(0)
enable_alter_motion(n=N_DVEL, mode=DR_DVEL, ref=DR_BASE,
                    limit_dPOS=LIM_POS, limit_dPOS_per=LIM_INC)
g_mode = 1
g_gain = 1.0
g_t0   = time.time()
th_id  = thread_run(alter_thread, loop=True)

movel(X2, vel=30, acc=60)                      # base motion with the sensor correction

g_gain = 0.0
thread_stop(th_id)
disable_alter_motion()

Related commands

• alter_motion()

• disable_alter_motion()

Keyword

enable / alter / motion / enable_ / enable_alter / enable_alter_