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