servol()

# servol() streaming example
#   Raises the TCP of the reference pose by 50 mm along Base +Z and returns.
#   The start position is not hard-coded, so this runs on any robot model.
#   The orientation is not changed, so it also holds on 4-axis models.
#   Before running, make sure nothing obstructs the 50 mm above the TCP.

# Stream design values
AMP    = 50.0     # travel [mm], Base +Z
STROKE = 2.0      # one-way time [sec]
PERIOD = 0.05     # target update period [sec]
STEPS  = 40       # steps per stroke ( = STROKE / PERIOD )

# Parameter derivation
#   For smoothstep s(u) = 3u^2 - 2u^3, max s' = 1.5 and max s'' = 6.
#     required velocity     = 1.5 * AMP / STROKE
#     required acceleration = 6.0 * AMP / STROKE^2
V_REQ   = 1.5 * AMP / STROKE
A_REQ   = 6.0 * AMP / (STROKE * STROKE)
VEL_LIN = 1.2 * V_REQ          # linear velocity [mm/s]
ACC_LIN = 2.0 * A_REQ          # linear acceleration [mm/s^2]

# vel and acc take two elements: [linear, angular].
# Do not copy the translational value into the rotational one. Even when the
# orientation is streamed as well, keep the angular velocity at around
# 20 deg/s or below.
VEL_ANG = 20.0                 # angular velocity [deg/s]
ACC_ANG = 100.0                # angular acceleration [deg/s^2]

# Stopping margin check, at the peak stream speed (u = 0.5):
#   remaining distance >= v * PERIOD + v^2 / (2 * acc)
if V_REQ * PERIOD + (V_REQ * V_REQ) / (2.0 * ACC_LIN) > AMP / 2.0:
    tp_log("insufficient stopping margin - increase AMP or STROKE")

# Move to the reference pose
set_velj(30)
set_accj(60)
movej(posj(0, 0, 90, 0, 90, 0), vel=30, acc=60)
mwait(0)

# Take the start position from the commanded value, only once.
# get_desired_posx() returns a posx, whereas get_current_posx() returns a
# (posx, sol) tuple, so the two are received differently.
# Using a measured value as the reference feeds the IK convergence residual
# into the orientation target, where it is amplified. Do not read it again
# inside the loop, even when the program is run repeatedly.
p0 = get_desired_posx()

# Turning off the execution line display removes the per-line overhead, so the
# actual period stays close to the specified value.
drl_report_line(OFF)

# Precompute the stream targets so that every loop iteration costs the same.
# The curve has zero velocity at both ends and each stroke ends at a stop, so
# the commanded velocity stays continuous.
leg_up = []
leg_dn = []
for k in range(1, STEPS + 1):
    u = float(k) / float(STEPS)
    s = 3.0 * u * u - 2.0 * u * u * u
    Xu = posx(p0)                         # copy - inherits ori_type/sol/turn
    Xu[2] = p0[2] + AMP * s               # Base Z only, 0 -> AMP
    leg_up.append(Xu)
    Xd = posx(p0)
    Xd[2] = p0[2] + AMP * (1.0 - s)       # AMP -> 0
    leg_dn.append(Xd)

# Streaming: the loop body is only servol() and wait().
for leg in [leg_up, leg_dn]:
    t_begin = get_system_time()
    for Xt in leg:
        servol(Xt, vel=[VEL_LIN, VEL_ANG], acc=[ACC_LIN, ACC_ANG])
        wait(PERIOD)
    t_finish = get_system_time()
    mwait(0)          # wait until the stop completes; do not use a fixed wait()
    t_span = t_finish - t_begin
    tp_log("actual period " + str(round(t_span / STEPS, 4)) + " s (specified " + str(PERIOD) + " s)")

drl_report_line(ON)