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)
