def __init__(self, node):
super(DetectCenterState, self).__init__(node)
self._x = 0
self._y = 0
self._z = 0
self._centered = False
self.bt = BlockTracker()
def __init__(self, node):
super(DetectEstimateState, self).__init__(node)
self.bt = BlockTracker()
self._ready = False
self._block_centered = False
self._got_image = False
self._z = None
class DetectEstimateState(State):
def __init__(self, node):
super(DetectEstimateState, self).__init__(node)
self.bt = BlockTracker()
self._ready = False
self._block_centered = False
self._got_image = False
self._z = None
############################################################################
def enter(self):
self._ready = False
self._block_centered = False
self._got_image = False
self._z = RobotStateMachine.calibrate.table_z + \
(RobotStateMachine.calibrate.block_height * 2)
self._node.ik.set_right(0.5, 0.0, self._z, wait=True)
self._ready = True
def next(self):
if not self._block_centered:
return self
else:
return RobotStateMachine.detect_center
############################################################################
def on_right_image_received(self, img):
if self._got_image or not self._ready:
return
self.bt.on_image_received(img)
self._got_image = True
if not self.bt.display_img is None:
self._node.display_image(self.bt.display_img)
self._block = random.choice(self.bt.blocks)
self._block_pos = RobotStateMachine.calibrate.calc_pos(
self._block.rel_pos[0], self._block.rel_pos[1])
self._node.ik.set_right(self._block_pos[0],
self._block_pos[1],
self._z,
wait=True)
self._block_centered = True
class DetectEstimateState(State):
def __init__(self, node):
super(DetectEstimateState, self).__init__(node)
self.bt = BlockTracker()
self._ready = False
self._block_centered = False
self._got_image = False
self._z = None
############################################################################
def enter(self):
self._ready = False
self._block_centered = False
self._got_image = False
self._z = RobotStateMachine.calibrate.table_z + \
(RobotStateMachine.calibrate.block_height * 2)
self._node.ik.set_right(0.5, 0.0, self._z, wait=True)
self._ready = True
def next(self):
if not self._block_centered:
return self
else:
return RobotStateMachine.detect_center
############################################################################
def on_right_image_received(self, img):
if self._got_image or not self._ready:
return
self.bt.on_image_received(img)
self._got_image = True
if not self.bt.display_img is None:
self._node.display_image(self.bt.display_img)
self._block = random.choice(self.bt.blocks)
self._block_pos = RobotStateMachine.calibrate.calc_pos(
self._block.rel_pos[0], self._block.rel_pos[1])
self._node.ik.set_right(self._block_pos[0], self._block_pos[1], self._z, wait=True)
self._block_centered = True
def __init__(self, node):
super(DetectCenterState, self).__init__(node)
self._x = 0
self._y = 0
self._z = 0
self._centered = False
self.bt = BlockTracker()
def __init__(self, node):
super(DetectEstimateState, self).__init__(node)
self.bt = BlockTracker()
self._ready = False
self._block_centered = False
self._got_image = False
self._z = None
def __init__(self, node):
super(CalibratePositionEstimationState, self).__init__(node)
self.bt = BlockTracker()
self._x = 0.5
self._y = 0
self._z = 0
self._target_keys = self._TARGETS.keys()
self._target = self._target_keys[0]
self._target_pos = dict([(x, None) for x in self._target_keys])
self._target_pos_calibrated = False
self._target_pos_computed = False
self.x_dif = None
self.y_dif = None
class DetectCenterState(State):
_MOVE_RATE = (1.0 / BlockStackerNode.RATE) * 0.1
_THRESHOLD = 0.05
############################################################################
def __init__(self, node):
super(DetectCenterState, self).__init__(node)
self._x = 0
self._y = 0
self._z = 0
self._centered = False
self.bt = BlockTracker()
############################################################################
def enter(self):
self._centered = False
self._x = RobotStateMachine.detect_estimate._block_pos[0]
self._y = RobotStateMachine.detect_estimate._block_pos[1]
self._z = RobotStateMachine.calibrate.table_z + \
(RobotStateMachine.calibrate.block_height * 2)
def run_step(self):
if not self._centered:
self._do_centering()
def next(self):
if not self._centered:
return self
else:
return RobotStateMachine.pick
############################################################################
def _do_centering(self):
if not len(self.bt.blocks) > 0:
return
target = (0.1, -0.5)
b = self.bt.blocks[0]
dif = (target[0] - b.rel_pos[0], target[1] - b.rel_pos[1])
if abs(dif[0]) > self._THRESHOLD or abs(dif[1]) > self._THRESHOLD:
if abs(dif[0]) > self._THRESHOLD:
self._x += math.copysign(self._MOVE_RATE, -dif[0]) * np.clip(abs(dif[0]), 0.1, 1.0)
if abs(dif[1]) > self._THRESHOLD:
self._y += math.copysign(self._MOVE_RATE, dif[1]) * np.clip(abs(dif[1]), 0.1, 1.0)
else:
self._centered = True
self._node.ik.set_right(self._x, self._y, self._z)
############################################################################
def on_right_image_received(self, img):
self.bt.on_image_received(img)
if not self.bt.display_img is None:
self._node.display_image(self.bt.display_img)
class CalibratePositionEstimationState(State):
_MOVE_RATE = (1.0 / BlockStackerCalibrateNode.RATE) * 0.1
_THRESHOLD = 0.05
_TARGET_DISTANCE = 0.5
_TARGETS = {
'center': (0, 0),
'left': (-_TARGET_DISTANCE, 0),
'right': (_TARGET_DISTANCE, 0),
'up': (0, -_TARGET_DISTANCE),
'down': (0, _TARGET_DISTANCE),
}
############################################################################
def __init__(self, node):
super(CalibratePositionEstimationState, self).__init__(node)
self.bt = BlockTracker()
self._x = 0.5
self._y = 0
self._z = 0
self._target_keys = self._TARGETS.keys()
self._target = self._target_keys[0]
self._target_pos = dict([(x, None) for x in self._target_keys])
self._target_pos_calibrated = False
self._target_pos_computed = False
self.x_dif = None
self.y_dif = None
############################################################################
def enter(self):
self._z = RobotStateMachine.calibrate_table_height.table_z + \
(RobotStateMachine.calibrate_block_height.block_height * 2)
print('estimating screen/arm positions...')
def run_step(self):
if not self._target_pos_calibrated:
self._do_target_pos_calibration()
elif not self._target_pos_computed:
self._do_target_pos_computation()
def next(self):
if not self._target_pos_calibrated:
return self
elif not self._target_pos_computed:
return self
else:
return RobotStateMachine.output_yaml
############################################################################
def _do_target_pos_calibration(self):
if not len(self.bt.blocks) > 0:
return
target = self._TARGETS[self._target]
b = self.bt.blocks[0]
dif = (target[0] - b.rel_pos[0], target[1] - b.rel_pos[1])
if abs(dif[0]) > self._THRESHOLD or abs(dif[1]) > self._THRESHOLD:
if abs(dif[0]) > self._THRESHOLD:
self._x += math.copysign(self._MOVE_RATE, -dif[0]) * np.clip(abs(dif[0]), 0.1, 1.0)
if abs(dif[1]) > self._THRESHOLD:
self._y += math.copysign(self._MOVE_RATE, dif[1]) * np.clip(abs(dif[1]), 0.1, 1.0)
else:
self._target_pos[self._target] = [self._x, self._y]
i = self._target_keys.index(self._target) + 1
if not i >= len(self._target_keys):
self._target = self._target_keys[i]
else:
self._target_pos_calibrated = True
print('got all positions')
self._node.ik.set_right(self._x, self._y, self._z)
def _do_target_pos_computation(self):
print('combobulating...')
l_dif = abs(self._target_pos['left'][0] - self._target_pos['center'][0])
r_dif = abs(self._target_pos['right'][0] - self._target_pos['center'][0])
u_dif = abs(self._target_pos['up'][1] - self._target_pos['center'][1])
d_dif = abs(self._target_pos['down'][1] - self._target_pos['center'][1])
self.x_dif = (l_dif + r_dif) / 2 / self._TARGET_DISTANCE
self.y_dif = (u_dif + d_dif) / 2 / self._TARGET_DISTANCE
self._target_pos_computed = True
############################################################################
def on_right_image_received(self, img):
self.bt.on_image_received(img)
if not self.bt.display_img is None:
self._node.display_image(self.bt.display_img)
class DetectCenterState(State):
_MOVE_RATE = (1.0 / BlockStackerNode.RATE) * 0.1
_THRESHOLD = 0.05
############################################################################
def __init__(self, node):
super(DetectCenterState, self).__init__(node)
self._x = 0
self._y = 0
self._z = 0
self._centered = False
self.bt = BlockTracker()
############################################################################
def enter(self):
self._centered = False
self._x = RobotStateMachine.detect_estimate._block_pos[0]
self._y = RobotStateMachine.detect_estimate._block_pos[1]
self._z = RobotStateMachine.calibrate.table_z + \
(RobotStateMachine.calibrate.block_height * 2)
def run_step(self):
if not self._centered:
self._do_centering()
def next(self):
if not self._centered:
return self
else:
return RobotStateMachine.pick
############################################################################
def _do_centering(self):
if not len(self.bt.blocks) > 0:
return
target = (0.1, -0.5)
b = self.bt.blocks[0]
dif = (target[0] - b.rel_pos[0], target[1] - b.rel_pos[1])
if abs(dif[0]) > self._THRESHOLD or abs(dif[1]) > self._THRESHOLD:
if abs(dif[0]) > self._THRESHOLD:
self._x += math.copysign(self._MOVE_RATE, -dif[0]) * np.clip(
abs(dif[0]), 0.1, 1.0)
if abs(dif[1]) > self._THRESHOLD:
self._y += math.copysign(self._MOVE_RATE, dif[1]) * np.clip(
abs(dif[1]), 0.1, 1.0)
else:
self._centered = True
self._node.ik.set_right(self._x, self._y, self._z)
############################################################################
def on_right_image_received(self, img):
self.bt.on_image_received(img)
if not self.bt.display_img is None:
self._node.display_image(self.bt.display_img)
