task2.py 文件源码

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.14

        log("Commanding hand above cable end")
        msg = ZarjMovePalmCommand('right', False, x, y, z, -1 * anchor.angle, 20, -90, True)
        self.fc.process_palm_msg(msg)
        self.done = True

        log("Giving the hand a long time to settle down.")
        rospy.sleep(3.0)