def pointRight(self, bottle): """ This method executes the point command with the right arm. @param bottle - Message Format: <near-far:double> <left-right:double> <down-up:double> """ # get the coordinates from the bottle near_far = bottle.get(0).asDouble() * -1.0 left_right = bottle.get(1).asDouble() * -1.0 down_up = bottle.get(2).asDouble() * -1.0 # calculate the angles point_to = [left_right, down_up, near_far] angle_d2 = rad2deg( angle((0.0, 1.0, 0.0), point_to) ) angle_d7 = 90 - rad2deg( angle((1.0, 0.0, 0.0), point_to) ) # split angle d4 into d4 and d5 values angle_d8 = min( 1, angle_d7) + 90 angle_d7 = max( 1, angle_d7) # send it to the motors self.ezb.setPosition(9, 90) self.ezb.setPosition(8, angle_d8) self.ezb.setPosition(7, angle_d7) self.ezb.setPosition(2, angle_d2) print angle_d2, angle_d7, angle_d8
def pointLeft(self, bottle): """ This method executes the point command with the left arm. @param bottle - Message Format: <near-far:double> <left-right:double> <down-up:double> """ # get the coordinates from the bottle near_far = bottle.get(0).asDouble() * -1.0 left_right = bottle.get(1).asDouble() * -1.0 down_up = bottle.get(2).asDouble() * -1.0 # calculate the angles point_to = [left_right, down_up, near_far] angle_d3 = 180 - rad2deg( angle((0.0, 1.0, 0.0), point_to) ) angle_d4 = 180 - (rad2deg( angle((1.0, 0.0, 0.0), point_to) ) - 90) # split angle d4 into d4 and d5 values angle_d5 = max( 0, angle_d4 - 180) + 90 angle_d4 = min(180, angle_d4) # send it to the motors self.ezb.setPosition(6, 90) self.ezb.setPosition(5, angle_d5) self.ezb.setPosition(4, angle_d4) self.ezb.setPosition(3, angle_d3) print angle_d3, angle_d4, angle_d5
def pointRight(self, bottle): """ This method executes the point command with the right arm. @param bottle - Message Format: <near-far:double> <left-right:double> <down-up:double> """ # get the coordinates from the bottle near_far = bottle.get(0).asDouble() * -1.0 left_right = bottle.get(1).asDouble() * -1.0 down_up = bottle.get(2).asDouble() * -1.0 # calculate the angles point_to = [left_right, down_up, near_far] angle_d2 = rad2deg(angle((0.0, 1.0, 0.0), point_to)) angle_d7 = 90 - rad2deg(angle((1.0, 0.0, 0.0), point_to)) # split angle d4 into d4 and d5 values angle_d8 = min(1, angle_d7) + 90 angle_d7 = max(1, angle_d7) # send it to the motors self.ezb.setPosition(9, 90) self.ezb.setPosition(8, angle_d8) self.ezb.setPosition(7, angle_d7) self.ezb.setPosition(2, angle_d2) print angle_d2, angle_d7, angle_d8
def pointLeft(self, bottle): """ This method executes the point command with the left arm. @param bottle - Message Format: <near-far:double> <left-right:double> <down-up:double> """ # get the coordinates from the bottle near_far = bottle.get(0).asDouble() * -1.0 left_right = bottle.get(1).asDouble() * -1.0 down_up = bottle.get(2).asDouble() * -1.0 # calculate the angles point_to = [left_right, down_up, near_far] angle_d3 = 180 - rad2deg(angle((0.0, 1.0, 0.0), point_to)) angle_d4 = 180 - (rad2deg(angle((1.0, 0.0, 0.0), point_to)) - 90) # split angle d4 into d4 and d5 values angle_d5 = max(0, angle_d4 - 180) + 90 angle_d4 = min(180, angle_d4) # send it to the motors self.ezb.setPosition(6, 90) self.ezb.setPosition(5, angle_d5) self.ezb.setPosition(4, angle_d4) self.ezb.setPosition(3, angle_d3) print angle_d3, angle_d4, angle_d5
def lookAt(self, bottle): """ This method executes the lookAt command. @param bottle - Message Format: <near-far:double> <left-right:double> <down-up:double> """ # get the coordinates from the bottle near_far = bottle.get(0).asDouble() * -1.0 left_right = bottle.get(1).asDouble() down_up = bottle.get(2).asDouble() # calculate the angles angle_d0 = rad2deg( angle([1.0, 0.0, 0.0], [left_right, down_up, near_far]) ) + 20 angle_d1 = rad2deg( angle([0.0, 1.0, 0.0], [left_right, down_up, near_far]) ) # send it to the motors self.ezb.setPosition(0, angle_d0) self.ezb.setPosition(1, angle_d1)
def lookAt(self, bottle): """ This method executes the lookAt command. @param bottle - Message Format: <near-far:double> <left-right:double> <down-up:double> """ # get the coordinates from the bottle near_far = bottle.get(0).asDouble() * -1.0 left_right = bottle.get(1).asDouble() down_up = bottle.get(2).asDouble() # calculate the angles angle_d0 = rad2deg( angle([1.0, 0.0, 0.0], [left_right, down_up, near_far])) + 20 angle_d1 = rad2deg( angle([0.0, 1.0, 0.0], [left_right, down_up, near_far])) # send it to the motors self.ezb.setPosition(0, angle_d0) self.ezb.setPosition(1, angle_d1)