def start(self, _=None):
""" Start the macro """
log("Starting the path - should get us to the exit")
self.fc.zarj.zps.path_start()
log("Walking out of the finish box")
self.fc.zarj.zps.dead_walk([0.6, 0.0, 0.0])
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 """
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 """
joints = deepcopy(LimbTypes.arm_styles['carry_array_higher'])
joints = LimbTypes.invert_arm_configuration('left', joints)
self.fc.zarj.hands.set_arm_configuration('left', joints)
rospy.sleep(0.5)
if self.fc.last_trailer_move is not None:
log("Walking back to trailer center by {}; back up a bit".format(
self.fc.last_trailer_move * -1))
log("TODO - maybe this should not be a deadwalk")
self.fc.zarj.zps.dead_walk(
[-0.06, 0.0, self.fc.last_trailer_move * -1])
self.fc.last_trailer_move = None
self.fc.clear_points()
self.fc.zarj.pelvis.turn_body_to(-45)
joints = deepcopy(LimbTypes.arm_styles['spin_array_1'])
joints = LimbTypes.invert_arm_configuration('left', joints)
self.fc.zarj.hands.set_arm_configuration('left', joints)
rospy.sleep(0.5)
self.fc.zarj.pelvis.turn_body_to(-75)
joints = deepcopy(LimbTypes.arm_styles['spin_array_2'])
joints = LimbTypes.invert_arm_configuration('left', joints)
self.fc.zarj.hands.set_arm_configuration('left', joints)
rospy.sleep(0.5)
joints = deepcopy(LimbTypes.arm_styles['spin_array_3'])
joints = LimbTypes.invert_arm_configuration('left', joints)
self.fc.zarj.hands.set_arm_configuration('left', joints)
rospy.sleep(0.5)
joints = deepcopy(LimbTypes.hand_styles['oppose'])
joints = LimbTypes.invert_hand_configuration('left', joints)
self.fc.zarj.hands.set_hand_configuration('left', joints)
rospy.sleep(0.5)
joints = deepcopy(LimbTypes.arm_styles['spin_array_clear'])
joints = LimbTypes.invert_arm_configuration('left', joints)
self.fc.zarj.hands.set_arm_configuration('left', joints)
rospy.sleep(0.5)
self.fc.zarj.pelvis.turn_body_to(-60)
joints = deepcopy(LimbTypes.arm_styles['king_tut'])
joints = LimbTypes.invert_arm_configuration('left', joints)
self.fc.zarj.hands.set_arm_configuration('left', joints)
rospy.sleep(0.5)
self.fc.zarj.pelvis.turn_body_to(0)
self.done = True
def start(self, _=None):
""" Start the macro """
log("Commanding hand to pinch cable")
joints = deepcopy(LimbTypes.hand_styles['grab_cable'])
joints = LimbTypes.invert_hand_configuration('right', joints)
self.fc.zarj.hands.set_hand_configuration('right', joints)
rospy.sleep(0.5) # A little extra time to let us get a good grip
self.done = True
def anchor_average(anchor, point1, point2, offset):
if point1 is not None and point2 is not None:
anchor.adjusted[0] = (point2[0] + point1[0]) / 2
anchor.adjusted[1] = (point2[1] + point1[1]) / 2
anchor.adjusted[2] = (point2[2] + point1[2]) / 2
if anchor.adjusted[1] != 0:
dx = point2[0] - point1[0]
dy = point2[1] - point1[1]
anchor.angle = math.degrees(math.atan(dx/dy))
return True
else:
log("center anchor requires two points.")
return False
def anchor_measure(anchor, point1, point2, offset):
if point1 is not None and point2 is not None:
anchor.adjusted[0] = point2[0] - point1[0]
anchor.adjusted[1] = point2[1] - point1[1]
anchor.adjusted[2] = point2[2] - point1[2]
if anchor.adjusted[1] != 0:
anchor.angle = math.degrees(math.atan(anchor.adjusted[0]/anchor.adjusted[1]))
else:
anchor.angle = 90.0
return True
else:
log("measure anchor requires two points.")
return False
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 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 """
joints = deepcopy(LimbTypes.arm_styles['pass_football'])
joints = LimbTypes.invert_arm_configuration('left', joints)
log("Setting left arm to pass a football")
self.fc.zarj.hands.set_arm_configuration('left', joints)
joints = deepcopy(LimbTypes.arm_styles['king_tut'])
joints = LimbTypes.invert_arm_configuration('right', joints)
log("king tut right arm, set to the bird")
self.fc.zarj.hands.set_arm_configuration('right', joints)
joints = deepcopy(LimbTypes.hand_styles['dabird'])
joints = LimbTypes.invert_hand_configuration('right', joints)
self.fc.zarj.hands.set_hand_configuration('right', joints)
log("Looking down and right; lean over")
self.fc.zarj.neck.neck_control([0.5, 0.0, 0], True)
self.fc.zarj.pelvis.lean_body_to(-30)
rospy.sleep(0.5)
self.fc.clear_points()
if not self.chain_okay:
self.fc.send_stereo_camera()
log("Click the inside cable chocke, 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.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("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, _=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 """
log("Lifting sequence; unlean, unturn, move left arm")
self.fc.zarj.pelvis.lean_body_to(0)
self.fc.zarj.pelvis.turn_body_to(0)
self.fc.zarj.neck.neck_control([0.5, 0.0, 0], True)
joints = deepcopy(LimbTypes.arm_styles['carry_array'])
joints = LimbTypes.invert_arm_configuration('left', joints)
self.fc.zarj.hands.set_arm_configuration('left', joints)
joints = deepcopy(LimbTypes.arm_styles['carry_array_middle'])
joints = LimbTypes.invert_arm_configuration('left', joints)
self.fc.zarj.hands.set_arm_configuration('left', joints)
self.done = True
def start(self, _=None):
""" Start the macro """
joints = deepcopy(LimbTypes.hand_styles['open'])
joints = LimbTypes.invert_hand_configuration('left', joints)
self.fc.zarj.hands.set_hand_configuration('left', joints)
log("Looking down and left")
self.fc.zarj.neck.neck_control([0.5, 1.0, 0], True)
rospy.sleep(0.5)
self.fc.clear_points()
if not self.chain_okay:
self.fc.send_stereo_camera()
log("Click the button.")
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 """
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("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("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 find_choke(macro):
macro.fc.send_stereo_camera()
image, details = macro.fc.zarj.eyes.get_cloud_image_with_details(
macro.fc.cloud)
things = Things(image, details, 2)
if things.choke_inner:
point = PointStamped()
point.header = macro.fc.cloud.header
det = details[things.choke_inner.position[1],
things.choke_inner.position[0]]
point.point.x = det[0]
point.point.y = det[1]
point.point.z = det[2]
inner = macro.fc.zarj.transform.tf_buffer.transform(
point, macro.fc.zarj.walk.lfname)
log("inner choke found at: {}/{}/{}".format(inner.point.x,
inner.point.y,
inner.point.z))
if things.choke_outer:
point = PointStamped()
point.header = macro.fc.cloud.header
det = details[things.choke_outer.position[1],
things.choke_outer.position[0]]
point.point.x = det[0]
point.point.y = det[1]
point.point.z = det[2]
outer = macro.fc.zarj.transform.tf_buffer.transform(
point, macro.fc.zarj.walk.lfname)
log("outer choke found at: {}/{}/{}".format(
outer.point.x, outer.point.y, outer.point.z))
macro.fc.points[0] = [outer.point.x, outer.point.y, outer.point.z]
macro.fc.points[1] = [inner.point.x, inner.point.y, inner.point.z]
return True
return False
def find_plug(macro):
macro.fc.send_stereo_camera()
image, details = macro.fc.zarj.eyes.get_cloud_image_with_details(
macro.fc.cloud)
things = Things(image, details, 2)
if things.power_plug:
point = PointStamped()
point.header = macro.fc.cloud.header
det = details[things.power_plug.position[1],
things.power_plug.position[0]]
point.point.x = det[0]
point.point.y = det[1]
point.point.z = det[2]
plug = macro.fc.zarj.transform.tf_buffer.transform(
point, macro.fc.zarj.walk.lfname)
log("plug at: {}/{}/{}".format(plug.point.x, plug.point.y,
plug.point.z))
macro.fc.points[0] = [plug.point.x, plug.point.y, plug.point.z]
macro.fc.points[1] = None
return True
return False
def start(self, _=None):
""" Start the macro """
self.fc.zarj.zps.look_forward()
if self.stop:
return
if self.fc.zarj.zps.state & zarj.zps.STATE_PRESTART:
self.fc.zarj.zps.path_start()
else:
self.fc.zarj.zps.return_to_waypoint()
if self.stop:
return
self.arms_home()
self.fc.zarj.zps.continue_from_poi()
if self.stop:
return
self.fc.zarj.zps.walk_to_next_finish_box()
if self.stop:
return
self.fc.zarj.zps.look_for_exit()
log("I am out of the Task1 Wilderness")
self.done = True
def find_button(macro):
macro.fc.send_stereo_camera()
image, details = macro.fc.zarj.eyes.get_cloud_image_with_details(
macro.fc.cloud)
things = Things(image, details, 2)
if things.array_button is not None:
point = PointStamped()
point.header = macro.fc.cloud.header
det = things.array_button.computed_center
point.point.x = det[0]
point.point.y = det[1]
point.point.z = det[2]
button = macro.fc.zarj.transform.tf_buffer.transform(
point, macro.fc.zarj.walk.lfname)
log("button found at: {}/{}/{}".format(button.point.x, button.point.y,
button.point.z))
macro.fc.points[0] = [button.point.x, button.point.y, button.point.z]
macro.fc.points[1] = None
return True
return False
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 """
joints = deepcopy(LimbTypes.arm_styles['pass_football'])
joints = LimbTypes.invert_arm_configuration('right', joints)
log("Setting right hand to pass a football")
self.fc.zarj.hands.set_arm_configuration('right', joints)
joints = deepcopy(LimbTypes.arm_styles['tuck'])
joints = LimbTypes.invert_arm_configuration('left', joints)
log("Tucking left arm, setting left hand to opposition")
self.fc.zarj.hands.set_arm_configuration('left', joints)
joints = deepcopy(LimbTypes.hand_styles['oppose'])
joints = LimbTypes.invert_hand_configuration('left', joints)
self.fc.zarj.hands.set_hand_configuration('left', joints)
log("Looking down")
self.fc.zarj.neck.neck_control([0.5, 0, 0], True)
rospy.sleep(0.5)
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("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("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 """
log("Testing macro 1!")
rospy.sleep(1.0)
self.done = True
def start(self, _=None):
""" Start the macro """
log("Testing macro 2!")
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
