def main():
global radius, theta
global datagram_pos
args = parse_args()
config = json.loads(jsmin(args.config.read()))
# zmqmsgbus subscribe
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source',
pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
theta = np.array(node.recv('/lidar/theta'))
node.register_message_handler('/lidar/theta', update_scan_theta)
datagram_pos = node.recv('/position')
node.register_message_handler('/position', update_scan_pos)
radius = np.array(node.recv('/lidar/radius'))
node.register_message_handler('/lidar/radius', lambda topic, message: update_scan_radius(topic, message, args, config, node))
print('receiving')
while True:
time.sleep(1)
def table_position_control_thread():
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source',
pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
time.sleep(1)
# dont' start before robot position was acquired at least once
robot_pos = node.recv('/position')
while True:
try:
robot_pos = node.recv('/position', timeout=0)
except queue.Empty:
pass
try:
setpoint = node.recv('/left-arm/table-setpoint', timeout=0)
setpoint = [int(setpoint[0])] + list(map_table_to_body_frame(*setpoint[1:], robot_pos=robot_pos))
node.publish('/left-arm/setpoint', setpoint)
except queue.Empty:
pass
try:
setpoint = node.recv('/right-arm/table-setpoint', timeout=0)
setpoint = [int(setpoint[0])] + list(map_table_to_body_frame(*setpoint[1:], robot_pos=robot_pos))
node.publish('/right-arm/setpoint', setpoint)
except queue.Empty:
pass
time.sleep(0.01)
def main():
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
position = PositionObject()
handler = lambda topic, msg: position.set(msg)
node.register_message_handler('/position', handler)
handler = lambda topic, msg: proximity_beacon_msg(node, position, msg)
node.register_message_handler('/proximity_beacon', handler)
while True:
time.sleep(1)
def main():
logging.basicConfig(level=logging.DEBUG)
global got_lidar_fix
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
pose = PositionEstimator()
o = node.recv('/odometry_raw')
pose.update_odometry(o)
pose.reset([0.5, 0.5, np.pi / 2])
pose_lock = threading.Lock()
def pose_reset_cb(reset_val):
with pose_lock:
pose.reset(reset_val)
node.register_service('/position/reset', pose_reset_cb)
t = threading.Thread(target=lidar_fix_led_thread, args=(node, ))
t.start()
while True:
try:
o = node.recv('/odometry_raw', timeout=0)
with pose_lock:
pose.update_odometry(o)
node.publish('/position', pose.get_position())
except queue.Empty:
pass
try:
l = node.recv('/lidar/position', timeout=0)
with pose_lock:
robot_position_lidar = l
robot_position_kalman = pose.get_position()
if (np.linalg.norm(
np.array(robot_position_lidar[0:2]) -
np.array(robot_position_kalman[0:2])) < 0.2 and abs(
periodic_error(robot_position_lidar[2] -
robot_position_kalman[2])) < 0.1):
got_lidar_fix = True
pose.update_lidar(robot_position_lidar)
node.publish('/position', pose.get_position())
else:
got_lidar_fix = False
except queue.Empty:
pass
time.sleep(0.01)
def main():
parser = argparse.ArgumentParser()
args = parser.parse_args()
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source',
pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
global pose
global pose_lock
pose = RobotPose(xy=[0,0],theta=0)
pose_lock = Lock()
node.register_message_handler('/position', odometry_msg_handler)
global target
global target_lock
target = None
target_lock = Lock()
node.register_message_handler('/waypoint', waypoint_msg_handler)
obstacle_list = ObstacleList()
handler = lambda topic, msg: obstacle_list.add(msg, time.time())
node.register_message_handler('/obstacle', handler)
time.sleep(1)
waypoint = WayPoint()
while True:
with target_lock:
tg = copy.copy(target)
if tg is not None:
with pose_lock:
pos = copy.copy(pose)
obstacles = obstacle_list.get(time.time())
if obstacle_avoidance_robot_should_stop(pos, tg, obstacles):
v_left, v_right = 0, 0
else:
v_left, v_right = waypoint.process(pos, tg)
node.call('/actuator/velocity', ['left-wheel', -v_left]) # left wheel velocity inversed
node.call('/actuator/velocity', ['right-wheel', v_right])
obstacle_list.remove_old(time.time())
time.sleep(1/waypoint.frequency)
def main():
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
global pub
global pub_lock
pub = SimpleRPCActuatorPublisher(MASTER_BOARD_MSG_ADDR)
print('publish messages to {}:{}'.format(*MASTER_BOARD_MSG_ADDR))
pub_lock = threading.Lock()
node.register_service('/actuator/state', get_state)
node.register_service('/actuator/voltage', update_voltage)
node.register_service('/actuator/position', update_position)
node.register_service('/actuator/velocity', update_velocity)
node.register_service('/actuator/torque', update_torque)
node.register_service('/actuator/trajectory', update_trajectory)
while 1:
time.sleep(0.02)
with pub_lock:
pub.publish(time.time())
def main():
parser = argparse.ArgumentParser("send parameters over zmqmsgbus")
parser.add_argument("service", help="service to call")
parser.add_argument("arg", help="YAML call arguments")
parser.add_argument('--in', dest='from_addr', default='ipc://ipc/source')
parser.add_argument('--out', dest='to_addr', default='ipc://ipc/sink')
args = parser.parse_args()
bus = zmqmsgbus.Bus(sub_addr=args.from_addr, pub_addr=args.to_addr)
node = zmqmsgbus.Node(bus)
while 1:
try:
a = yaml.load(args.arg)
print('calling {} with {}'.format(args.service, a))
ret = node.call(args.service, a)
print(ret)
break
except zmqmsgbus.call.CallFailed as e:
print(e)
time.sleep(0.1)
def main():
parser = argparse.ArgumentParser(
"Sends the robot config to the master board.")
parser.add_argument("config", help="YAML file containing robot config.")
parser.add_argument("-w",
"--watch",
help="Watch config file for changes.",
action="store_true")
args = parser.parse_args()
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
# wait until services list received
time.sleep(1)
send_config_file(node, open(args.config))
if args.watch:
print("> watching for file changes...")
old_mtime = os.path.getmtime(args.config)
while True:
try:
try:
mtime = os.path.getmtime(args.config)
# Some editors delete the file before writing to it
except FileNotFoundError:
pass
if mtime != old_mtime:
old_mtime = mtime
send_config_file(node, open(args.config))
time.sleep(0.1)
except KeyboardInterrupt:
break
except:
logging.exception("Unexpected error occured.")
import zmqmsgbus
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
print('receiving')
while 1:
print(node.recv('/example/counter'))
def setUp(self, thread_mock):
self.bus = Mock()
self.node = zmqmsgbus.Node(self.bus)
def main():
logging.basicConfig(level=logging.DEBUG)
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
time.sleep(4)
logging.info("ready")
zero_torque(node)
team_color = None
while team_color is None:
team_color = init_sequence(node)
wait_for_start(node)
threading.Thread(target=kill_after_90_seconds, args=(node, )).start()
logging.info("start!")
# evade goldorak
goto_waypoint(node, team_color, [0.66, 0.18, pi / 2])
time.sleep(2)
# position behind blocks
move_arm_in_body_frame(node, team_color, 'right',
[0, 0.19, -0.06, 0.02, 0])
move_arm_in_body_frame(node, team_color, 'left', [0, 0.19, 0.06, 0.02, 0])
goto_waypoint(node, team_color, [0.9, 0.29, pi / 2])
time.sleep(0.5)
# push blocks
goto_waypoint(node, team_color, [0.9, 1.0, pi / 2])
time.sleep(0.5)
goto_waypoint(node, team_color, [0.9, 0.97, pi / 2])
time.sleep(0.5)
goto_waypoint(node, team_color, [0.9, 0.94, pi / 2])
move_arm_in_body_frame(node, team_color, 'right',
[0, 0.19, -0.06, 0.15, 0])
move_arm_in_body_frame(node, team_color, 'left', [0, 0.19, 0.06, 0.15, 0])
# safe_arm_position(node)
time.sleep(1)
goto_waypoint(node, team_color, [0.9, 0.50, pi])
# close door 1
goto_waypoint(node, team_color, [0.4, 0.3, -pi])
set_waypoint(node, team_color, [0.23, 0.3, -pi])
time.sleep(2)
goto_waypoint(node, team_color, [0.4, 0.3, 0])
# close door 2
goto_waypoint(node, team_color, [0.4, 0.6, -pi])
set_waypoint(node, team_color, [0.23, 0.6, -pi])
time.sleep(2)
goto_waypoint(node, team_color, [0.4, 0.6, pi])
# get second block of elements
goto_waypoint(node, team_color, [0.35, 0.9, pi / 2])
time.sleep(1)
# sweep off tower
move_arm_in_table_frame(node, team_color, 'left',
[0, 0.068, 0.84, 0.14, 0])
set_pump(node, team_color, 'left', 1, 12)
set_pump(node, team_color, 'left', 2, 12)
set_pump(node, team_color, 'left', 3, 12)
set_pump(node, team_color, 'left', 4, 12)
time.sleep(2)
# move above
move_arm_in_table_frame(node, team_color, 'left',
[0, 0.068, 0.88, 0.14, 0])
time.sleep(1)
# grab blocks
move_arm_in_table_frame(node, team_color, 'left',
[0, 0.068, 0.88, 0.12, 0])
time.sleep(1)
# move up
move_arm_in_table_frame(node, team_color, 'left',
[0, 0.068, 0.88, 0.21, 0])
time.sleep(1)
# move arm behind
move_arm_in_body_frame(node, team_color, 'left', [0, -.1, 0, 0.21, 0])
time.sleep(1)
# turn around
goto_waypoint(node, team_color, [0.35, 0.86, -pi / 2])
time.sleep(1)
# do sweep movement to be sure
move_arm_in_table_frame(node, team_color, 'right',
[0, 0.068, 0.76, 0.13, 0])
time.sleep(2)
move_arm_in_table_frame(node, team_color, 'right',
[0, 0.068, 0.76, 0.08, 0])
set_pump(node, team_color, 'right', 1, 12)
set_pump(node, team_color, 'right', 2, 12)
set_pump(node, team_color, 'right', 3, 12)
set_pump(node, team_color, 'right', 4, 12)
time.sleep(1)
# move above
move_arm_in_table_frame(node, team_color, 'right',
[0, 0.068, 0.88, 0.08, 0])
time.sleep(1)
# grab blocks
move_arm_in_table_frame(node, team_color, 'right',
[0, 0.068, 0.88, 0.06, 0])
time.sleep(1)
# move up
move_arm_in_table_frame(node, team_color, 'right',
[0, 0.068, 0.88, 0.21, 0])
time.sleep(1)
# move arm in front
move_arm_in_body_frame(node, team_color, 'right',
[0, 0.18, -0.03, 0.21, 0])
time.sleep(1)
# drop right
goto_waypoint(node, team_color, [0.6, 1, pi / 2])
move_arm_in_table_frame(node, team_color, 'right', [0, 0.85, 1, 0.21, 0])
time.sleep(1)
move_arm_in_table_frame(node, team_color, 'right', [0, 0.85, 1, 0.065, 0])
time.sleep(1)
set_pump(node, team_color, 'right', 1, -12)
set_pump(node, team_color, 'right', 2, -12)
set_pump(node, team_color, 'right', 3, -12)
set_pump(node, team_color, 'right', 4, -12)
time.sleep(1)
set_pump(node, team_color, 'right', 1, 0)
set_pump(node, team_color, 'right', 2, 0)
set_pump(node, team_color, 'right', 3, 0)
set_pump(node, team_color, 'right', 4, 0)
move_arm_in_body_frame(node, team_color, 'right',
[0, 0.18, -0.03, 0.21, 0])
time.sleep(1)
# drop left
goto_waypoint(node, team_color, [0.6, 1, -pi / 2])
move_arm_in_table_frame(node, team_color, 'left', [0, 0.85, 1, 0.21, 0])
time.sleep(1)
move_arm_in_table_frame(node, team_color, 'left', [0, 0.85, 1, 0.13, 0])
time.sleep(1)
set_pump(node, team_color, 'left', 1, -12)
set_pump(node, team_color, 'left', 2, -12)
set_pump(node, team_color, 'left', 3, -12)
set_pump(node, team_color, 'left', 4, -12)
time.sleep(1)
set_pump(node, team_color, 'left', 1, 0)
set_pump(node, team_color, 'left', 2, 0)
set_pump(node, team_color, 'left', 3, 0)
set_pump(node, team_color, 'left', 4, 0)
move_arm_in_body_frame(node, team_color, 'left', [0, -.1, 0, 0.21, 0])
time.sleep(1)
# drop all elements
time.sleep(10)
set_pump(node, team_color, 'left', 1, 0)
set_pump(node, team_color, 'left', 2, 0)
set_pump(node, team_color, 'left', 3, 0)
set_pump(node, team_color, 'left', 4, 0)
set_pump(node, team_color, 'right', 1, 0)
set_pump(node, team_color, 'right', 2, 0)
set_pump(node, team_color, 'right', 3, 0)
set_pump(node, team_color, 'right', 4, 0)
def test_connect_service_socket_tcp_random_port(self, thd):
bus = Mock()
bus.ctx.socket.return_value.bind_to_random_port.return_value = 1234
node = zmqmsgbus.Node(bus, serv_addr='tcp://localhost')
self.assertEqual('tcp://localhost:1234', node.serv_addr)
def test_connect_service_socket_given_addr(self, thd):
bus = Mock()
node = zmqmsgbus.Node(bus, serv_addr='tcp://localhost:123')
self.assertEqual('tcp://localhost:123', node.serv_addr)
def test_connect_service_socket_no_addr(self, thd, uuid):
uuid.return_value = 123456
bus = Mock()
node = zmqmsgbus.Node(bus, serv_addr=None)
self.assertEqual('ipc://ipc/123456', node.serv_addr)
def __init__(self):
self.bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source',
pub_addr='ipc://ipc/sink')
self.node = zmqmsgbus.Node(self.bus)
def main():
logging.basicConfig(level=logging.DEBUG)
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source',
pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
time.sleep(1)
offsets = yaml.load(open(sys.argv[1]))
left_arm_config = yaml.load(open("config/config-left-arm.yaml"))
right_arm_config = yaml.load(open("config/config-right-arm.yaml"))
global l, r, arm_lock
arm_lock = threading.Lock()
l = Arm(negative_elbow_angle=True)
r = Arm()
def zero_homing(arg):
global l, r, arm_lock
logging.debug('perform zero homing')
with arm_lock:
endstopper = homing_handler.Endstopper()
endstopper.add(['left-z'], 30, left_arm_config['actuator']['left-z']['control']['torque_limit'])
endstopper.add(['right-z'], 30, right_arm_config['actuator']['right-z']['control']['torque_limit'])
endstopper_zeros = endstopper.start()
l.set_zeros({a[len('left-'):]: z * offsets[a + '-dir'] for a, z in endstopper_zeros.items() if 'left-' in a})
r.set_zeros({a[len('right-'):]: z * offsets[a + '-dir'] for a, z in endstopper_zeros.items() if 'right-' in a})
def zero_sequence(side):
global l, r
if side == 'left':
arm = l
else:
arm = r
arm.set_zeros({'shoulder': cst_vel_homing.homing(side+'-shoulder', 2)
* offsets[side+'-shoulder-dir']})
arm.set_zeros({'elbow': cst_vel_homing.homing(side+'-elbow', 2)
* offsets[side+'-elbow-dir']})
arm.set_zeros({'wrist': cst_vel_homing.homing(side+'-wrist', 2, periodic=True)
* offsets[side+'-wrist-dir']})
left = threading.Thread(target=zero_sequence, args=('left',))
left.start()
right = threading.Thread(target=zero_sequence, args=('right',))
right.start()
left.join()
right.join()
l.move_hand(0.212, 0, 0.18, 0)
r.move_hand(0.212, 0, 0.18, 0)
logging.debug('left hand zeros: {}'.format(l.zeros))
logging.debug('right hand zeros: {}'.format(r.zeros))
actuator_position(node, 'left', l.get_actuators(), offsets)
actuator_position(node, 'right', r.get_actuators(), offsets)
node.register_service('/arm/run_zero_homing', zero_homing)
table_pos_control = threading.Thread(target=table_position_control_thread)
table_pos_control.start()
while True:
try:
setpoint = node.recv('/left-arm/setpoint', timeout=0)
with arm_lock:
setpoint = [int(setpoint[0])] + list(map_body_to_arm_frame(*[float(v) for v in setpoint[1:]], arm='left'))
if setpoint[0] == 0:
l.move_hand(*setpoint[1:])
else:
l.move_tcp(*setpoint)
actuator_position(node, 'left', l.get_actuators(), offsets)
except queue.Empty:
pass
except ValueError as e:
logging.debug('left arm ' + str(e))
try:
setpoint = node.recv('/right-arm/setpoint', timeout=0)
with arm_lock:
setpoint = [int(setpoint[0])] + list(map_body_to_arm_frame(*[float(v) for v in setpoint[1:]], arm='right'))
if setpoint[0] == 0:
r.move_hand(*setpoint[1:])
else:
r.move_tcp(*setpoint)
actuator_position(node, 'right', r.get_actuators(), offsets)
except queue.Empty:
pass
except ValueError as e:
logging.debug('right arm ' + str(e))
time.sleep(0.01)
def main():
global position, heading
global cloud_pts, cloud_pts_plot
global red_cloud_pts
global convexHulls
global segments
global ext_segments
global corners
global features
args = parse_args()
config = json.loads(jsmin(args.config.read()))
# zmqmsgbus subscribe
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source',
pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
print('Waiting for registration to finish...')
# position = node.recv('/lidar_testing/position')
# node.register_message_handler('/lidar_testing/position', update_position)
print('.')
cloud_pts = node.recv('/lidar_viewer/cloud_pts')
node.register_message_handler('/lidar_viewer/cloud_pts', update_cloud_pts)
print('.')
red_cloud_pts = node.recv('/lidar_viewer/red_cloud_pts')
node.register_message_handler('/lidar_viewer/red_cloud_pts', update_red_cloud_pts)
print('.')
segments = node.recv('/lidar_viewer/segments')
node.register_message_handler('/lidar_viewer/segments', update_segments)
print('.')
ext_segments = node.recv('/lidar_viewer/ext_segments')
node.register_message_handler('/lidar_viewer/ext_segments', update_ext_segments)
print('.')
corners = node.recv('/lidar_viewer/corners')
node.register_message_handler('/lidar_viewer/corners', update_corners)
print('.')
features = node.recv('/lidar_viewer/features')
node.register_message_handler('/lidar_viewer/features', update_features)
print('-- ok')
# PyQtGraph stuff
app = QtGui.QApplication([])
pg.setConfigOptions(antialias=False)
plot = pg.plot(title='Lidar Polar Plot')
plot.resize(600,600)
plot.setAspectLocked()
print('Plotting...')
while 1:
time.sleep(1)
plot.clear()
PlotPolar(plot)
cloud_size = red_cloud_pts.shape[0]
plot.plot(red_cloud_pts[np.arange(0,cloud_size,4),0],red_cloud_pts[np.arange(0,cloud_size,4),1],pen=None,symbol='o', symbolPen=None,symbolSize=7,symbolBrush=(255,255,255,50))
linePen = pg.mkPen(color=(200,200,200,200),width=2,
style=QtCore.Qt.DotLine)
colors=[(255,0,0,180),(0,255,0,180),(0,0,255,180),(255,255,0,180),
(255,0,255,180),(0,127,255,180),(127,0,0,180),(0,127,0,180),
(0,0,127,180),(127,255,0,180),(127,0,255,180),(0,127,255,180)]
# # draw segments
# try:
# for idx in range(segments.shape[0]):
# linePen = pg.mkPen(color=colors[idx],width=4,
# style=QtCore.Qt.SolidLine)
# a = plot.plot((segments[idx][0][0], segments[idx][0][1]),
# (segments[idx][1][0], segments[idx][1][1]), pen=linePen)
# except:
# pass
# draw external segments
try:
for idx in range(ext_segments.shape[0]):
linePen = pg.mkPen(color=colors[idx],width=4,
style=QtCore.Qt.SolidLine)
a = plot.plot((ext_segments[idx][0][0], ext_segments[idx][0][1]),
(ext_segments[idx][1][0], ext_segments[idx][1][1]), pen=linePen)
except:
pass
# draw corners found
symbolePen = pg.mkPen(color=(255,255,0,255), width=4)
if corners.shape[0] > 0:
plot.plot(corners[:,0], corners[:,1], pen=None, symbol='x',
symbolPen=symbolePen)
# draw found feature
# symbolePen = pg.mkPen(color=(255,255,0,255), width=4)
# print(features.shape)
# if features.shape[0] > 0:
# plot.plot(features[:,0], features[:,1], pen=None, symbol='x',
# symbolPen=symbolePen)
# # draw table estimation corner
# table_corner = np.array([[0,0],[0,config['TABLE_HEIGHT']],[config['TABLE_WIDTH'],
# config['TABLE_HEIGHT']],[config['TABLE_WIDTH'],0]], dtype=float)
# table_corner = table_corner - [position[0], position[1]]
# table_corner = rotatePolygon(table_corner, -heading+math.pi/2)
# symbolePen = pg.mkPen(color=(255,255,255,255), width= 2)
# plot.plot(table_corner[:,0], table_corner[:,1],
# pen=None, symbol='x', symbolPen=symbolePen, symbolSize=12)
app.processEvents()
QtGui.QApplication.instance().exec_()
def homing(actuator, velocity, periodic=False):
bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink')
node = zmqmsgbus.Node(bus)
time.sleep(1)
node.call('/actuator/velocity', [actuator, 0])
node.call(
'/actuator/config_update',
{'actuator': {
actuator: {
'stream': {
'index': 10,
'motor_pos': 30
}
}
}})
state = {
'vel': 0,
'idx_p': None,
'idx_n': None,
'idx_seq_nbr': float('inf')
}
def vel_cb(topic, msg):
state['vel'] = msg
def idx_cb(topic, msg):
seq = msg[1]
if seq > state['idx_seq_nbr']: # new index
if abs(state['vel']) > velocity * 0.1:
if state['vel'] > 0:
state['idx_p'] = msg[0]
else:
state['idx_n'] = msg[0]
state['idx_seq_nbr'] = seq
node.register_message_handler('/actuator/' + actuator + '/velocity',
vel_cb)
node.register_message_handler('/actuator/' + actuator + '/index', idx_cb)
n = 0.3
while True:
node.call('/actuator/velocity', [actuator, velocity])
time.sleep(n)
if state['idx_p'] is not None and state['idx_n'] is not None:
break
node.call('/actuator/velocity', [actuator, -velocity])
time.sleep(2 * n)
if state['idx_p'] is not None and state['idx_n'] is not None:
break
node.call('/actuator/velocity', [actuator, velocity])
time.sleep(n)
if state['idx_p'] is not None and state['idx_n'] is not None:
break
n *= 2
if periodic:
center = center_between_periodic_angles(state['idx_p'], state['idx_n'])
else:
center = (state['idx_p'] + state['idx_n']) / 2
node.call('/actuator/position', [actuator, center])
node.call(
'/actuator/config_update',
{'actuator': {
actuator: {
'stream': {
'index': 0,
'motor_pos': 0
}
}
}})
return center
