def start(self, previous=None):
""" Start the macro """
if previous and hasattr(previous, 'sat_location'):
self.sat_location = previous.sat_location
else:
raise ZarjConfused('No satallite location known')
log("====> Satellite Movement Plan:\n\t{}m side\n\t {}m ahead".format(
self.sat_location[0], self.sat_location[1] - APPROACH_DISTANCE))
if self.sat_location[1] - APPROACH_DISTANCE > 0.25:
raise (ZarjConfused(
"OPERATOR! headed off the path trying to approach the satellite Not right!"
))
if abs(self.sat_location[0]) > 0.01:
self.fc.zarj.walk.side(self.sat_location[0])
zarj.utils.wait_for_walk(self.fc.zarj)
if self.stop:
return
if abs(self.sat_location[1] - APPROACH_DISTANCE) > 0.01:
self.fc.zarj.walk.forward(self.sat_location[1] - APPROACH_DISTANCE)
zarj.utils.wait_for_walk(self.fc.zarj)
if self.stop:
return
self._lean_in()
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("drop_array")
if anchor is None:
raise ZarjConfused("Cannot find the choke")
joints = deepcopy(LimbTypes.arm_styles['release_array_1'])
joints = LimbTypes.invert_arm_configuration('left', joints)
log("Setting left arm to hold it out")
self.fc.zarj.hands.set_arm_configuration('left', joints)
rospy.sleep(0.5)
log("Lowering hand")
msg = ZarjMovePalmCommand('left', False, 0.81, 0.15, 1.16, 0.0, 0.0,
-90.0, True)
self.fc.process_palm_msg(msg)
joints = deepcopy(LimbTypes.hand_styles['oppose'])
joints = LimbTypes.invert_hand_configuration('left', joints)
log("Opening hand")
self.fc.zarj.hands.set_hand_configuration('left', joints)
rospy.sleep(0.25)
log("Retracting hand")
msg = ZarjMovePalmCommand('left', False, 0.61, 0.15, 1.16, 0.0, 0.0,
-90.0, True)
self.fc.process_palm_msg(msg)
if self.fc.task != 2 or self.fc.checkpoint < 3:
raise ZarjConfused("Checkpoint did not progress")
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("center")
if anchor is None:
raise ZarjConfused("Cannot find the choke")
log("Lifting cable; unlean, unturn, move hands")
x = anchor.adjusted[0] + .1
y = anchor.adjusted[1] + .1
z = anchor.adjusted[2] + 0.3
log("Commanding hand into the air")
msg = ZarjMovePalmCommand('right', False, x, y, z, 0, 0, 45, True)
self.fc.process_palm_msg(msg)
self.fc.zarj.neck.neck_control([0.5, 0, 0], True)
self.fc.clear_points()
if not self.chain_okay:
self.fc.send_stereo_camera()
log("Click the center of the power plug.")
if self.fc.task != 2 or self.fc.checkpoint < 5:
raise ZarjConfused("Checkpoint did not progress")
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("connect_cable")
if anchor is None:
raise ZarjConfused("Cannot find the plug")
log("Moving feet to plug in anchor")
forward = anchor.adjusted[0]
sideways = anchor.adjusted[1]
# This is generally a move bacwkards; let's do that before
# the step to the side, to keep the cord from tangling
log("Walking forward {}".format(forward))
self.fc.zarj.zps.dead_walk([forward, 0.0, 0.0])
log("Walking sideways {}".format(sideways))
self.fc.zarj.zps.dead_walk([0.0, 0.0, sideways])
log("TODO - maybe this should not be a deadwalk")
log("Look down")
self.fc.zarj.neck.neck_control([0.5, .0, 0], True)
log("Lean and turn forward")
self.fc.zarj.pelvis.lean_body_to(-30)
self.fc.zarj.pelvis.turn_body_to(0)
if not self.chain_okay:
self.fc.send_stereo_camera()
log("Click the power plug")
self.done = True
def start(self, _=None):
""" Start the macro """
self._prepare_to_turn()
self.fc.zarj.zps.look_down()
#have the look_down settle
rospy.sleep(1)
log('closer look')
_, blue, _ = self.find_satellite()
retries = 5
while blue is None and retries > 0:
rospy.sleep(0.5)
_, blue, _ = self.find_satellite()
retries -= 1
if blue is None:
raise ZarjConfused('Unable to find blue control!')
y_center = blue[1].point.y + OFFSET_BLUE
x_front = blue[0].point.x
log('satellite {} over, {} ahead'.format(y_center, x_front))
self.sat_location = (y_center, x_front)
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("pickup_cable")
if anchor is None:
raise ZarjConfused("Cannot find the choke")
log("Array is {} away, {} to the side, at angle {}".format(
anchor.adjusted[0], anchor.adjusted[1], anchor.angle))
forward = anchor.adjusted[0]
sideways = anchor.adjusted[1]
log("Walking sideways {}, forward {}".format(sideways, forward))
log("TODO - maybe this should not be a deadwalk")
self.fc.zarj.zps.dead_walk([forward, 0.0, sideways])
log("Look right")
self.fc.zarj.neck.neck_control([0.5, -1.0, 0], True)
log("Lean and turn")
self.fc.zarj.pelvis.lean_body_to(-30)
self.fc.zarj.pelvis.turn_body_to(30)
if not self.chain_okay:
self.fc.send_stereo_camera()
log("Click the inside cable choke, and then the outer choke, in that order."
)
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("task2_prep_button")
if anchor is None:
raise ZarjConfused("Cannot find the button")
x = anchor.adjusted[0]
y = anchor.adjusted[1]
z = anchor.adjusted[2] + 0.1
log("Commanding hand to go back up over release array")
msg = ZarjMovePalmCommand('left', False, x, y, z, 0, 0, 0, True)
self.fc.process_palm_msg(msg)
if self.fc.task != 2 or self.fc.checkpoint < 4:
raise ZarjConfused("Checkpoint did not progress")
self.done = True
def start(self, _=None):
""" Start the macro """
if self.fc.task == 0 and self.chain_okay:
raise ZarjConfused("OPERATOR ERROR: You asked to start satellite "
"stuff but we are still task 0, so srcsim has "
"not been started! I dont think so!")
self.fc.zarj.zps.look_forward()
self.sat_location = self._locate_satellite(False)
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("plugin_finish")
if anchor is None:
raise ZarjConfused("Cannot find the plug")
x = anchor.adjusted[0]
y = anchor.adjusted[1]
z = anchor.adjusted[2]
log("Commanding hand to plug er in; x {}, y {}, z {}".format(x, y, z))
msg = ZarjMovePalmCommand('right', False, x, y, z, 0, 0, 45, True)
self.fc.process_palm_msg(msg)
self.done = True
log("Give it a while.")
rospy.sleep(3.0)
if self.fc.task != 2 or self.fc.checkpoint < 6:
raise ZarjConfused("Checkpoint did not progress")
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("center")
if anchor is None:
raise ZarjConfused("Cannot find the array")
x = anchor.adjusted[0] + (.06 * math.cos(math.radians(anchor.angle)))
y = anchor.adjusted[1] - (.06 * math.sin(math.radians(anchor.angle)))
z = anchor.adjusted[2] - 0.045
log("Commanding hand to position under handle")
msg = ZarjMovePalmCommand('left', False, x, y, z, -1 * anchor.angle, 0,
-90, True)
self.fc.process_palm_msg(msg)
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("task2_prep_button")
if anchor is None:
raise ZarjConfused("Cannot find the button")
x = anchor.adjusted[0]
y = anchor.adjusted[1]
z = anchor.adjusted[2] - 0.15
log("Commanding hand onto button")
msg = ZarjMovePalmCommand('left', False, x, y, z, 0, 0, 0, True)
self.fc.process_palm_msg(msg)
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("center")
if anchor is None:
raise ZarjConfused("Cannot find the choke")
x = anchor.adjusted[0]
y = anchor.adjusted[1] + (.05 * math.sin(math.radians(anchor.angle)))
z = anchor.adjusted[2] + 0.04
log("Commanding hand onto cable end")
msg = ZarjMovePalmCommand('right', False, x, y, z, -1 * anchor.angle,
20, -90, True)
self.fc.process_palm_msg(msg)
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("plugin_pre_start")
if anchor is None:
raise ZarjConfused("Cannot find the plug")
x = anchor.adjusted[0]
y = anchor.adjusted[1]
z = anchor.adjusted[2]
log("Commanding hand above and left of plug; x {}, y {}, z {}".format(
x, y, z))
msg = ZarjMovePalmCommand('right', False, x, y, z, 0, 0, 45, True)
self.fc.process_palm_msg(msg)
self.done = True
def start(self, previous=None):
""" Start the macro """
if previous and hasattr(previous, 'sat_location'):
self.sat_location = previous.sat_location
else:
raise ZarjConfused('No satallite location known')
self.fc.zarj.zps.set_poi((self.sat_location[0], self.sat_location[1]))
# walk to dish
self.fc.zarj.zps.walk_to_poi()
if self.stop:
return
self.fc.zarj.zps.mark_waypoint()
if self.stop:
return
# second walk if needed
if self.sat_location[2] > 0:
self.fc.zarj.walk.forward(self.sat_location[2])
zarj.utils.wait_for_walk(self.fc.zarj)
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("plugin_finish")
if anchor is None:
raise ZarjConfused("Cannot find the plug")
x = anchor.adjusted[0]
y = anchor.adjusted[1]
z = anchor.adjusted[2] + .2
log("Flipping bird, commanding hand to let go; x {}, y {}, z {}".
format(x, y, z))
joints = deepcopy(LimbTypes.hand_styles['purebird'])
joints = LimbTypes.invert_hand_configuration('right', joints)
self.fc.zarj.hands.set_hand_configuration('right', joints)
msg = ZarjMovePalmCommand('right', False, x, y, z, 0, 0, -90, True)
self.fc.process_palm_msg(msg)
self.done = True
self.fc.clear_points()
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("drop_array")
if anchor is None:
raise ZarjConfused("Cannot find the choke")
log("Array is {} away, {} to the side, at angle {}".format(
anchor.adjusted[0], anchor.adjusted[1], anchor.angle))
forward = anchor.adjusted[0]
sideways = anchor.adjusted[1]
log("Walking sideways {}, forward {}".format(sideways, forward))
log("TODO - maybe this should not be a deadwalk")
self.fc.zarj.zps.dead_walk([forward, 0.0, sideways])
self.fc.clear_points()
if not self.chain_okay:
self.fc.send_stereo_camera()
log("Pick the inner choke, followed by the outer choke")
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("center")
if anchor is None:
raise ZarjConfused("Cannot find the array")
log("Array is {} away, {} to the side, at angle {}".format(
anchor.adjusted[0], anchor.adjusted[1], anchor.angle))
forward = anchor.adjusted[0] - 0.71
if anchor.angle > 0:
self.fc.last_trailer_move = .17
else:
self.fc.last_trailer_move = -.17
sideways = anchor.adjusted[1] + self.fc.last_trailer_move
log("Walking sideways {}, forward {}".format(sideways, forward))
log("TODO - maybe this should not be a deadwalk")
self.fc.zarj.zps.dead_walk([forward, 0.0, sideways])
log("Lean down -30, turn -40, look left")
self.fc.zarj.pelvis.lean_body_to(-30)
self.fc.zarj.neck.neck_control([0.5, 0.75, 0], True)
self.fc.zarj.pelvis.turn_body_to(-40)
if anchor.angle > 0:
joints = deepcopy(LimbTypes.arm_styles['tuck_out'])
joints = LimbTypes.invert_arm_configuration('left', joints)
log("Swinging left arm out, to make the move easier.")
self.fc.zarj.hands.set_arm_configuration('left', joints)
rospy.sleep(0.5)
if not self.chain_okay:
self.fc.send_stereo_camera()
log("Pick the two top corners of the array handle.")
self.done = True
def start(self, _=None):
""" Start the macro """
anchor = self.fc.assure_anchor("drop_array")
if anchor is None:
raise ZarjConfused("Cannot find the choke")
log("Set left arm to carry higher, right to football")
joints = deepcopy(LimbTypes.arm_styles['carry_array_higher'])
joints = LimbTypes.invert_arm_configuration('left', joints)
self.fc.zarj.hands.set_arm_configuration('left', joints)
joints = deepcopy(LimbTypes.arm_styles['pass_football'])
joints = LimbTypes.invert_arm_configuration('right', joints)
self.fc.zarj.hands.set_arm_configuration('right', joints)
sideways = anchor.adjusted[1]
if sideways > 0:
log("Stepping left {} before swinging array".format(sideways))
log("TODO - maybe this should not be a deadwalk")
self.fc.zarj.zps.dead_walk([0.0, 0.0, sideways])
joints = deepcopy(LimbTypes.arm_styles['release_array_1'])
joints = LimbTypes.invert_arm_configuration('left', joints)
log("Setting left arm to hold it out")
self.fc.zarj.hands.set_arm_configuration('left', joints)
rospy.sleep(0.5)
log("Lean down, move neck so we can see choke")
self.fc.zarj.pelvis.lean_body_to(-15)
self.fc.zarj.pelvis.turn_body_to(-45)
self.fc.zarj.neck.neck_control([0.5, 0.0, 0], True)
if not self.chain_okay:
self.fc.send_stereo_camera()
log("Pick the inner choke, followed by the outer choke")
self.done = True
def _locate_satellite(self, retry=False):
""" Locate the satellite dish """
red, blue, final = self.find_satellite(2, True)
retries = 5
while (red is None or blue is None) and retries > 0:
rospy.sleep(0.5)
red, blue, final = self.find_satellite(2, True)
retries -= 1
if red is None or blue is None:
raise ZarjConfused('Could not find both red and blue wheels')
side = 0
if final[0].point.y < 0:
log('controls are on the right')
side = -1
else:
log('controls are on the left')
side = 1
blue_center_y = (blue[0].point.y + blue[1].point.y) / 2
blue_center_x = (blue[0].point.x + blue[1].point.x) / 2
red_center_y = (red[0].point.y + red[1].point.y) / 2
red_center_x = (red[0].point.x + red[1].point.x) / 2
y_offset = round(abs(blue_center_y - red_center_y), 3)
x_offset = round(abs(blue_center_x - red_center_x), 3)
if y_offset == 0.0:
y_offset = 0.001
if x_offset == 0.0:
x_offset = 0.001
ratio = x_offset / y_offset
log('y_offset = {}'.format(y_offset))
log('x_offset = {}'.format(x_offset))
log('distance = {}'.format(x_offset + y_offset))
log('ratio = {}'.format(ratio))
if x_offset + y_offset < 0.2:
if not retry:
log('Controls are not in a logical position, too close... '
'Retry')
return self._locate_satellite(True)
else:
raise ZarjConfused(
'Controls are not in a logical position, too close')
# This is not really rocket science:
# 45 should have ratios around 1
# 90 has ratios above above 3 even as high as 400
# ahead would have ratios approaching 0
if np.isclose([x_offset], [y_offset])[0] or \
(ratio >= 0.6 and ratio <= 3.0):
log('controls are 45 degrees')
turn = 45
correction = -0.7
second = 0.3
elif round((blue_center_y + red_center_y) / 2, 2) == 0.0:
log('controls are directly ahead')
turn = 0
correction = -0.5
second = 0.0
else:
log('controls are 90 degrees')
turn = 90
correction = 0.0
second = 0.3
final_center_x = (final[0].point.x + final[1].point.x) / 2
log("Satellite Plan: distance {}, turn {}, approach {}".format(
final_center_x + correction, turn * side, second))
return final_center_x + correction, turn * side, second
def _correct_alignment(self, msg):
""" Correct satellite alignment """
if not msg.pitch_correct_now:
log("Correcting pitch: target pitch {} current pitch {} ".format(
msg.target_pitch, msg.current_pitch))
if msg.current_pitch - msg.target_pitch > 0:
self.rotate_to_top('right')
movement = abs((msg.current_pitch - self.last_pitch))
if movement < WHEEL_MOVEMENT_THRESHOLD and self.last_pitch != 0:
if movement == 0:
log("Wheel did not move at all!")
self.pitch_fail_count += 5
else:
log("Wheel only moved {}, retry {} of {}!".format(
movement, self.pitch_fail_count,
THRESHOLD_FAIL_COUNT))
self.pitch_fail_count += 1
if self.pitch_fail_count > THRESHOLD_FAIL_COUNT:
# Not moving wheel, fix it!
log("Wheel not moving; last pitch {} current pitch {} "
"movement {}".format(self.last_pitch,
msg.current_pitch, movement))
raise ZarjConfused('Wheel not moving')
else:
self.pitch_fail_count = 0
else:
self.rotate_to_side('right')
movement = abs((self.last_pitch - msg.current_pitch))
if movement < WHEEL_MOVEMENT_THRESHOLD and self.last_pitch != 0:
if movement == 0:
log("Wheel did not move at all!")
self.pitch_fail_count += 5
else:
log("Wheel only moved {}, retry {} of {}!".format(
movement, self.pitch_fail_count,
THRESHOLD_FAIL_COUNT))
self.pitch_fail_count += 1
if self.pitch_fail_count > THRESHOLD_FAIL_COUNT:
# Not moving wheel, fix it!
log("Wheel not moving; last pitch {} current pitch {} "
"movement {}".format(self.last_pitch,
msg.current_pitch, movement))
raise ZarjConfused('Wheel not moving')
else:
self.pitch_fail_count = 0
self.last_pitch = msg.current_pitch
if not msg.yaw_correct_now:
log("Correcting yaw: target yaw {} current yaw {} ".format(
msg.target_yaw, msg.current_yaw))
if msg.current_yaw - msg.target_yaw < 0:
self.rotate_to_top('left')
movement = abs((self.last_yaw - msg.current_yaw))
if movement < WHEEL_MOVEMENT_THRESHOLD and self.last_yaw != 0:
if movement == 0:
log("Wheel did not move at all!")
self.yaw_fail_count += 5
else:
log("Wheel only moved {}, retry {} of {}!".format(
movement, self.yaw_fail_count,
THRESHOLD_FAIL_COUNT))
self.yaw_fail_count += 1
if self.yaw_fail_count > THRESHOLD_FAIL_COUNT:
# Not moving wheel, fix it!
log("Wheel not moving; last yaw {} current yaw {} "
"movement {}".format(self.last_yaw,
msg.current_yaw, movement))
raise ZarjConfused('Wheel not moving')
else:
self.yaw_fail_count = 0
else:
self.rotate_to_side('left')
movement = abs((self.last_yaw - msg.current_yaw))
if movement < WHEEL_MOVEMENT_THRESHOLD and self.last_yaw != 0:
if movement == 0:
log("Wheel did not move at all!")
self.yaw_fail_count += 5
else:
log("Wheel only moved {}, retry {} of {}!".format(
movement, self.yaw_fail_count,
THRESHOLD_FAIL_COUNT))
self.yaw_fail_count += 1
if self.yaw_fail_count > THRESHOLD_FAIL_COUNT:
# Not moving wheel, fix it!
log("Wheel not moving; last yaw {} current yaw {} "
"movement {}".format(self.last_yaw,
msg.current_yaw, movement))
raise ZarjConfused('Wheel not moving')
else:
self.yaw_fail_count = 0
self.last_yaw = msg.current_yaw
def next_chain(self):
""" returns the name of the next macro in the chain """
if len(self.fc.zarj.zps.waypoint_stack) == 0:
raise ZarjConfused("OPERATOR: We cannot chain")
return 'task1_finish_box'
def _find_control(self,
tag,
mask,
details,
header,
factor,
other_one=None):
""" Find a control """
corners = [None, None]
indexes = mask.nonzero()
points = []
indexes2 = []
for i in range(len(indexes[1])):
pnt = details[indexes[0][i], indexes[1][i]]
if pnt[2] > DISH_MAXIMUM:
print 'rejecting point, too distant ', pnt[2]
mask[indexes[0][i], indexes[1][i]] = 0
continue
if other_one:
dist = np.sqrt((other_one[0].point.y - pnt[0])**2 + \
(other_one[0].point.z - pnt[1])**2 +\
(other_one[0].point.x - pnt[2])**2)
if dist > 1.0:
print 'rejecting point, too far away from other one', dist
mask[indexes[0][i], indexes[1][i]] = 0
continue
points.append(details[indexes[0][i], indexes[1][i]])
indexes2.append((indexes[0][i], indexes[1][i]))
points = np.array(points)
if len(points) < 3:
log("Error: only {} {} points found.".format(len(points), tag))
raise ZarjConfused('Error: only {} {} points found.'.format(
len(points), tag))
mean = np.mean(points, axis=0)[2]
stdv = np.std(points, axis=0)[2]
for i, pnt in enumerate(points):
if abs(pnt[2] - mean) < factor * stdv:
_add_bounding_point(pnt, corners)
else:
mask[indexes[0][i], indexes[1][i]] = 0
point = PointStamped()
point.header = header
point.point.x = corners[0][0]
point.point.y = corners[0][1]
point.point.z = corners[0][2]
cor1 = self.fc.zarj.transform.tf_buffer.transform(point, 'pelvis')
point = PointStamped()
point.header = header
point.point.x = corners[1][0]
point.point.y = corners[1][1]
point.point.z = corners[1][2]
cor2 = self.fc.zarj.transform.tf_buffer.transform(point, 'pelvis')
log("{}, {} {}".format(tag, cor1, cor2))
return (cor1, cor2)
def start(self, _=None):
""" Start the macro """
if not find_plug(self):
raise ZarjConfused("Cannot find the plug")
else:
self.done = True