Definition
servoj(pos, vel, acc, time, mod)
Features
The command is the asynchronous motion command, and the next command is executed at the same time the motion begins. When the mode is set to Override mode, it follows the most recent target joint position in a motion within the maximum speed and acceleration among the continuously delivered commands. When the mode is set to Queue mode, it can store up to 100 previous commands and sequentially follow the path. When 100 waypoints have been stored in Queue mode, the command will not return until reaching the next waypoint. If an Override command is input during Queue mode, it ignores the previously stored waypoints and follows the most recent target joint position.
Parameters
|
Parameter Name |
Data Type |
Default Value |
Description |
|---|---|---|---|
|
pos |
posj |
- |
posj or joint angle list |
|
list (float[6]) |
|||
|
vel (v) |
float |
None |
maximum velocity (same to all axes) or maximum velocity (to an axis) [deg/s] |
|
list (float[6]) |
|||
|
acc (a) |
float |
None |
maximum acceleration (same to all axes) or maximum acceleration (acceleration to an axis) [deg/s2] |
|
list (float[6]) |
|||
|
time (t) |
float |
None |
reach time [sec] |
|
mod |
int |
DR_SERVO_OVERRIDE |
Waypoint mode
|
Note
-
Abbreviated parameter names are supported. (v:vel, a:acc, t:time)
-
_global_velj is applied if vel is None. (The initial value of _global_velj is 0.0 and can be set by set_velj.)
-
_global_accj is applied if acc is None. (The initial value of _global_accj is 0.0 and can be set by set_accj.)
-
After time is set, If reach time can’t be keep because of condition of maximum velocity and acceleration, the reach time is adjusted automatically and notice through information message.
Caution
-
It is not linked with the speed control function of the speed slide bar.
-
It is not linked with check_motion(), change_operation_speed() functions.
-
It is not linked with the Speed Reduction Ratio setting of the Safety Zone.
Return
|
Value |
Description |
|---|---|
|
0 |
Success |
|
Negative value |
Error |
Exception
|
Exception |
Description |
|---|---|
|
DR_Error (DR_ERROR_TYPE) |
Parameter data type error occurred |
|
DR_Error (DR_ERROR_VALUE) |
Parameter value is invalid |
|
DR_Error (DR_ERROR_RUNTIME) |
C extension module error occurred |
|
DR_Error (DR_ERROR_STOP) |
Program terminated forcefully |
Example
# servoj() streaming example
# Moves J1 by +20 deg from the reference pose and returns.
# The start pose is not hard-coded, so this runs on any robot model.
# Before running, make sure nothing obstructs the J1 rotation path.
# Stream design values
AXIS = 0 # joint to stream (0 = J1)
AMP = 20.0 # amplitude [deg]
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
# vel is 1.2x the required velocity, acc is 2x the required acceleration.
# Setting vel too high makes the robot reach, stop and restart on every
# command, which increases vibration instead of reducing it.
V_REQ = 1.5 * AMP / STROKE
A_REQ = 6.0 * AMP / (STROKE * STROKE)
VEL = 1.2 * V_REQ
ACC = 2.0 * A_REQ
# 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) > 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 pose from the commanded value.
# get_current_posj() is a measured value, so its tracking error would be fed
# back into the stream targets.
q0 = get_desired_posj()
# 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_fwd = []
leg_rev = []
for k in range(1, STEPS + 1):
u = float(k) / float(STEPS)
s = 3.0 * u * u - 2.0 * u * u * u
Xf = posj(q0) # copy - does not modify q0
Xf[AXIS] = q0[AXIS] + AMP * s # 0 -> AMP
leg_fwd.append(Xf)
Xr = posj(q0)
Xr[AXIS] = q0[AXIS] + AMP * (1.0 - s) # AMP -> 0
leg_rev.append(Xr)
# Streaming: the loop body is only servoj() and wait().
for leg in [leg_fwd, leg_rev]:
t_begin = get_system_time()
for Xt in leg:
servoj(Xt, vel=VEL, acc=ACC)
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)