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 move_palm_cb(self, button): msg = ZarjMovePalmCommand(self.movepalm_side_combo.get_active_text(), self.relative_checkbox.get_active(), float(self.handx.get_text()), float(self.handy.get_text()), float(self.handz.get_text()), float(self.palmyaw.get_text()), float(self.palmpitch.get_text()), float(self.palmroll.get_text()), True) self.zarj_comm.push_message(msg)
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("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 """ 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("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("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