def _actuator_handler(self):
"""Sends actuation commands to UR5.
Waits for `actuator_buffer` being updated. If the connection
is lost, does not do anything. Otherwise, proceeds with
sending the command corresponding to the content of
`actuator_buffer` with an added delay. The delay is randomly
exponentially distributed with scale set by self._delay attribute."""
# sleep until ready to send an actuation command
# the ready condition depends on the `actuator_sync_period`
while self._actuator_running:
if self._actuation_sync_period > 0:
# Synchronized to sensor readings, so check to see if the correct
# number of readings have been detected to break out of the loop
self._num_read_lock.acquire()
if self._num_reads >= self._actuation_sync_period:
self._num_reads = 0
self._num_read_lock.release()
break
self._num_read_lock.release()
else:
if self.actuator_buffer.updated():
break
time.sleep(0.0001)
# Do not perform an actuation if the socket connection is broken
# or the actuator should no longer be running
if self._stop or not self._actuator_running:
return
# obtain actuation command if it is ready
# or a read was just performed and an action needs to be sent
sent_count = 0
for entry in self._commands_queue:
cmd_str, timestamp, delay = entry
now = time.time()
if now - timestamp > delay:
self._last_command = cmd_str
self._sent_times.append(now)
if not 'speedj' in str(self._last_command):
self._sock.send(cmd_str)
sent_count += 1
time.sleep(0.0001)
else:
break
self._commands_queue = self._commands_queue[sent_count:]
if not self._last_command is None and 'speedj' in str(
self._last_command):
self._sock.send(self._last_command)
if self.actuator_buffer.updated(
) or self._actuation_sync_period > 0 or self._delay > 0.0:
# keep track of the updated flag as it will be set to 0 below
updated = self.actuator_buffer.updated()
self._num_read_lock.acquire()
self._num_reads = 0
self._num_read_lock.release()
recent_actuation, time_stamp, _ = self.actuator_buffer.read_update(
)
recent_actuation = recent_actuation[0]
if recent_actuation[0] == ur_utils.COMMANDS['SERVOJ']['id']:
servoj_values = ur_utils.COMMANDS['SERVOJ']
cmd = ur_utils.ServoJ(
q=recent_actuation[1:1 + servoj_values['size'] - 3],
t=servoj_values['default']['t'] if recent_actuation[-3]
== ur_utils.USE_DEFAULT else recent_actuation[-3],
lookahead_time=servoj_values['default']['lookahead_time']
if recent_actuation[-2] == ur_utils.USE_DEFAULT else
recent_actuation[-2],
gain=servoj_values['default']['gain']
if recent_actuation[-1] == ur_utils.USE_DEFAULT else
recent_actuation[-1])
elif recent_actuation[0] == ur_utils.COMMANDS['SPEEDJ']['id']:
if time.time() - time_stamp[-1] > self._speedj_timeout:
return
speedj_values = ur_utils.COMMANDS['SPEEDJ']
cmd = ur_utils.SpeedJ(
qd=recent_actuation[1:1 + speedj_values['size'] - 2],
a=speedj_values['default']['a'] if recent_actuation[-2]
== ur_utils.USE_DEFAULT else recent_actuation[-2],
t_min=speedj_values['default']['t_min']
if recent_actuation[-1] == ur_utils.USE_DEFAULT else
recent_actuation[-1],
)
elif not updated:
# The commands below this point should only be executed
# if they are new actuations
return
elif recent_actuation[0] == ur_utils.COMMANDS['MOVEL']['id']:
movel_values = ur_utils.COMMANDS['MOVEL']
cmd = ur_utils.MoveL(
pose=recent_actuation[1:1 + movel_values['size'] - 4],
a=movel_values['default']['a'] if recent_actuation[-4]
== ur_utils.USE_DEFAULT else recent_actuation[-4],
v=movel_values['default']['v'] if recent_actuation[-3]
== ur_utils.USE_DEFAULT else recent_actuation[-3],
t=movel_values['default']['t'] if recent_actuation[-2]
== ur_utils.USE_DEFAULT else recent_actuation[-2],
r=movel_values['default']['r'] if recent_actuation[-1]
== ur_utils.USE_DEFAULT else recent_actuation[-1],
)
elif recent_actuation[0] == ur_utils.COMMANDS['MOVEJ']['id']:
movej_values = ur_utils.COMMANDS['MOVEJ']
cmd = ur_utils.MoveJ(
q=recent_actuation[1:1 + movej_values['size'] - 4],
a=movej_values['default']['a'] if recent_actuation[-4]
== ur_utils.USE_DEFAULT else recent_actuation[-4],
v=movej_values['default']['v'] if recent_actuation[-3]
== ur_utils.USE_DEFAULT else recent_actuation[-3],
t=movej_values['default']['t'] if recent_actuation[-2]
== ur_utils.USE_DEFAULT else recent_actuation[-2],
r=movej_values['default']['r'] if recent_actuation[-1]
== ur_utils.USE_DEFAULT else recent_actuation[-1],
)
elif recent_actuation[0] == ur_utils.COMMANDS['STOPJ']['id']:
cmd = ur_utils.StopJ(a=recent_actuation[1])
elif recent_actuation[0] == ur_utils.COMMANDS['UNLOCK_PSTOP'][
'id']:
print("Unlocking p-stop")
self._dashboard_sock.send(
'unlock protective stop\n'.encode('ascii'))
return
elif recent_actuation[0] == ur_utils.COMMANDS['NOTHING']['id']:
return
else:
raise NotImplementedError
cmd_str = '{}\n'.format(cmd)
self._commands_queue.append([
cmd_str.encode('ascii'),
time.time(),
np.minimum(
int(np.random.exponential(scale=self._delay) / 0.008) *
0.008, 0.32)
])
def _actuator_handler(self):
"""Sends actuation commands to UR5.
Waits for `actuator_buffer` being updated.
If the connection is lost, does not do anything.
Otherwise, proceeds with sending the command corresponding to
the content of `actuator_buffer`.
"""
# sleep until ready to send an actuation command
# the ready condition depends on the `actuator_sync_period`
while self._actuator_running:
if self._actuation_sync_period > 0:
# Synchronized to sensor readings, so check to see if the correct
# number of readings have been detected to break out of the loop
self._num_read_lock.acquire()
if self._num_reads >= self._actuation_sync_period:
self._num_reads = 0
self._num_read_lock.release()
break
self._num_read_lock.release()
else:
# Not synchronized to sensor readings, so break out of the loop
# and actuate a command as soon as the buffer is updated
if self.actuator_buffer.updated():
break
time.sleep(1e-4)
# Do not perform an actuation if the socket connection is broken
# or the actuator should no longer be running
if self._stop or not self._actuator_running:
return
# obtain actuation command if it is ready
# or a read was just performed and an action needs to be sent
if self.actuator_buffer.updated() or self._actuation_sync_period > 0:
# keep track of the updated flag as it will be set to 0 below
updated = self.actuator_buffer.updated()
self._num_read_lock.acquire()
self._num_reads = 0
self._num_read_lock.release()
recent_actuation, time_stamp, _ = self.actuator_buffer.read_update()
recent_actuation = recent_actuation[0]
if recent_actuation[0] == ur_utils.COMMANDS['SERVOJ']['id']:
servoj_values = ur_utils.COMMANDS['SERVOJ']
cmd = ur_utils.ServoJ(
q=recent_actuation[1:1 + servoj_values['size'] - 3],
t=servoj_values['default']['t']
if recent_actuation[-3] == ur_utils.USE_DEFAULT else recent_actuation[-3],
lookahead_time=servoj_values['default']['lookahead_time']
if recent_actuation[-2] == ur_utils.USE_DEFAULT else recent_actuation[-2],
gain=servoj_values['default']['gain']
if recent_actuation[-1] == ur_utils.USE_DEFAULT else recent_actuation[-1]
)
elif recent_actuation[0] == ur_utils.COMMANDS['SPEEDJ']['id']:
if time.time() - time_stamp[-1] > self._speedj_timeout:
return
speedj_values = ur_utils.COMMANDS['SPEEDJ']
cmd = ur_utils.SpeedJ(
qd=recent_actuation[1:1 + speedj_values['size'] - 2],
a=speedj_values['default']['a']
if recent_actuation[-2] == ur_utils.USE_DEFAULT else recent_actuation[-2],
t_min=speedj_values['default']['t_min']
if recent_actuation[-1] == ur_utils.USE_DEFAULT else recent_actuation[-1],
)
elif not updated:
# The commands below this point should only be executed
# if they are new actuations
return
elif recent_actuation[0] == ur_utils.COMMANDS['MOVEL']['id']:
movel_values = ur_utils.COMMANDS['MOVEL']
cmd = ur_utils.MoveL(
pose=recent_actuation[1:1 + movel_values['size'] - 4],
a=movel_values['default']['a']
if recent_actuation[-4] == ur_utils.USE_DEFAULT else recent_actuation[-4],
v=movel_values['default']['v']
if recent_actuation[-3] == ur_utils.USE_DEFAULT else recent_actuation[-3],
t=movel_values['default']['t']
if recent_actuation[-2] == ur_utils.USE_DEFAULT else recent_actuation[-2],
r=movel_values['default']['r']
if recent_actuation[-1] == ur_utils.USE_DEFAULT else recent_actuation[-1],
)
elif recent_actuation[0] == ur_utils.COMMANDS['MOVEJ']['id']:
movej_values = ur_utils.COMMANDS['MOVEJ']
cmd = ur_utils.MoveJ(
q=recent_actuation[1:1 + movej_values['size'] - 4],
a=movej_values['default']['a']
if recent_actuation[-4] == ur_utils.USE_DEFAULT else recent_actuation[-4],
v=movej_values['default']['v']
if recent_actuation[-3] == ur_utils.USE_DEFAULT else recent_actuation[-3],
t=movej_values['default']['t']
if recent_actuation[-2] == ur_utils.USE_DEFAULT else recent_actuation[-2],
r=movej_values['default']['r']
if recent_actuation[-1] == ur_utils.USE_DEFAULT else recent_actuation[-1],
)
elif recent_actuation[0] == ur_utils.COMMANDS['STOPJ']['id']:
cmd = ur_utils.StopJ(
a=recent_actuation[1]
)
elif recent_actuation[0] == ur_utils.COMMANDS['UNLOCK_PSTOP']['id']:
print("Unlocking p-stop")
self._dashboard_sock.send('unlock protective stop\n'.encode('ascii'))
return
elif recent_actuation[0] == ur_utils.COMMANDS['NOTHING']['id']:
return
else:
raise NotImplementedError
cmd_str = '{}\n'.format(cmd)
self._sock.send(cmd_str.encode('ascii'))