from naoqi import ALProxy
args = sys.argv
IP = args[1]
PORT = 9559
scene = args[2]
# motion = ALProxy('ALMotion',IP,PORT)
try:
motion = ALProxy("ALMotion", IP, PORT)
except Exception as e:
quit()
motion.positionErrorThresholdPos = 0.0
motion.positionErrorThresholdAng = 0.0
def MoveTo(distX, distY, rotate):
print "Move To:"
print distX
print distY
print rotate
import almath
initPosition = almath.Pose2D(motion.getRobotPosition(True))
targetDistance = almath.Pose2D(distX, distY, rotate * almath.PI / 180)
expectedEndPosition = initPosition * targetDistance
enableArms = False
