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