def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
motionProxy.wakeUp()
postureProxy.goToPosture("StandInit", 0.5)
audioProxy = ALProxy("ALAudioDevice", robotIP, PORT)
audioProxy.setOutputVolume(45)
time.sleep(2)
motionProxy.setFallManagerEnabled(False)
t = Thread(target=CoMBalancing)
t.setDaemon(True)
t.start()
initArmPose("129.125.178.114", 9559) ### Could disturb balance
runtime = time.time()
useSensorValues = True
result = motionProxy.getRobotPosition(useSensorValues)
print "Robot Position", result
# Example showing how to use this information to know the robot's diplacement.
initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
vector = robotMove.toVector()
### Use while loop and check for robotPosition
while abs(vector[0]) < 1.5:
i = 0
print i
step(robotIP)
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
vector = robotMove.toVector()
#for i in range(6):
# step(robotIP)
runtime = time.time() - runtime
data = {'xCom': xList, 'b1_custom': accList, 'b2_custom': polyList, 'b1': accList1, 'b2': polyList1, 'acc': forceList, 'time': yList, 'runtime': runtime, 'distance': vector[0]}
### trial_default_X (With box, no disturbance)
### 10 trials for default
### trial_disturbed_x (With box, disturbed)
### 0 trial for disturbed
with open('trial_disturbed_4', 'wb') as f:
pickle.dump(data, f)
print "Time taken: ", runtime, " (s)"
motionProxy.rest()
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
useSensorValues = False
result = motionProxy.getRobotPosition(useSensorValues)
print "Robot Position", result
# Example showing how to use this information to know the robot's diplacement.
useSensorValues = False
initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
# Make the robot move
motionProxy.moveTo(0.1, 0.0, 0.2)
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
print "Robot Move:", robotMove
# Go to rest position
motionProxy.rest()
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send NAO to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
result = motionProxy.getNextRobotPosition()
print "Next Robot Position", result
# Example showing how to use this information to know the robot's diplacement
# during the move process.
# Make the robot move
motionProxy.post.moveTo(0.6, 0.0, 0.5) # No blocking due to post called
time.sleep(1.0)
initRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())
# Make the robot move
motionProxy.moveTo(0.1, 0.0, 0.2)
endRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
print "Robot Move :", robotMove
# Go to rest position
motionProxy.rest()
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send NAO to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
result = motionProxy.getNextRobotPosition()
print "Next Robot Position", result
# Example showing how to use this information to know the robot's diplacement
# during the move process.
# Make the robot move
motionProxy.post.moveTo(0.6, 0.0, 0.5) # No blocking due to post called
time.sleep(1.0)
initRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())
# Make the robot move
motionProxy.moveTo(0.1, 0.0, 0.2)
endRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
print "Robot Move :", robotMove
# Go to rest position
motionProxy.rest()
def main():
motionProxy = getProxy(Proxies.Motion)
postureProxy = getProxy(Proxies.RobotPosture)
# --- Defaults ----
speed = 0.5
useSensorValues = False
# Stand Init
postureProxy.goToPosture("StandInit", speed)
# Get position in the world
result = motionProxy.getRobotPosition(False)
print "Robot Postition", result
# Get robot displacement
initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
print "Init Robot Position", initRobotPosition
motionProxy.moveTo(0.5, 0.0, radians(280))
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
print "End Position", endRobotPosition
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
print "Robot move", robotMove
def TakeClosePic(self):
# x – normalized, unitless, velocity along X-axis.
# +1 and -1 correspond to the maximum velocity in the forward and backward directions, respectively.
x = 1.0
y = 0.0
theta = 0.0
# index of image
image_i = 0
frequency = 0.1
# get current time
t0 = time.time()
initRobotPosition = almath.Pose2D(
self.motion_service.getRobotPosition(False))
# since now the robot is 1.5 away from the human
# let the robot move 0.7 meters forward while taking photos(now the velocity is 0.1 meter per second)
# in order to keep 0.8 meter distance away from human
while ((time.time() - t0) <= 7.0):
# once 7s has been passed, break the loop
if (time.time() - t0) > 7.0:
break
# print "time difference :" + str((time.time() - t0))
self.motion_service.moveToward(x, y, theta,
[["MaxVelXY", frequency]])
image_i += 1
self.GetImage(image_i)
# stop the robot
self.motion_service.stopMove()
endRobotPosition = almath.Pose2D(
self.motion_service.getRobotPosition(False))
robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Distance change and angle change after approaching the human:", robotMove
def NaviageToTarget(self):
# navigation_service = self.session.service("ALNavigation")
try:
Xmove = self.Xlast
# if the distance (along x-axis) that the robot needs to move not equal to 0
if (Xmove != None):
initRobotPosition = almath.Pose2D(
self.motion_service.getRobotPosition(False))
# navigation_service.navigateTo(Xmove,0)
print(
"Distance (along x-axis) that the robot needs to move \n" +
"(in meter) to approach a human:", Xmove)
self.motion_service.moveTo(Xmove, 0, 0, _async=True)
self.motion_service.waitUntilMoveIsFinished()
time.sleep(0.5)
endRobotPosition = almath.Pose2D(
self.motion_service.getRobotPosition(False))
robotMove = almath.pose2DInverse(
initRobotPosition) * endRobotPosition
print(
"Distance change and angle change after moveTo() function:",
robotMove)
except KeyboardInterrupt:
print "Interrupted by user, stopping moving"
#stop
self.motion_service.stopMove()
sys.exit(0)
def main(session):
"""
Move To: Small example to make Nao Move To an Objective.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by move algorithm
#####################
motion_service.setMoveArmsEnabled(True, True)
#~ motion_service.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
X = 0.3
Y = 0.1
Theta = math.pi/2.0
motion_service.moveTo(X, Y, Theta, _async=True)
# wait is useful because with _async moveTo is not blocking function
motion_service.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# Go to rest position
motion_service.rest()
def main(session):
"""
Move To: Small example to make Nao Move To an Objective.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by move algorithm
#####################
motion_service.setMoveArmsEnabled(True, True)
#~ motion_service.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
X = 0.3
Y = 0.1
Theta = math.pi/2.0
motion_service.moveTo(X, Y, Theta, _async=True)
# wait is useful because with _async moveTo is not blocking function
motion_service.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# Go to rest position
motion_service.rest()
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Stand Init
postureProxy.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by move algorithm
#####################
motionProxy.setMoveArmsEnabled(True, True)
#~ motionProxy.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
X = 0.0
Y = 0.0
Theta = math.pi/2.0
motionProxy.post.moveTo(X, Y, Theta)
motionProxy.setAngles("HeadYaw", 0.6, 0.6)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = m.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = m.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# Go to rest position
motionProxy.rest()
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Stand Init
postureProxy.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by move algorithm
#####################
motionProxy.setMoveArmsEnabled(True, True)
#~ motionProxy.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
X = 0.3
Y = 0.1
Theta = math.pi / 2.0
motionProxy.post.moveTo(X, Y, Theta)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = m.pose2DInverse(initRobotPosition) * endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = m.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# Go to rest position
motionProxy.rest()
def calculate(self):
self.temp_position = self.get_position()
robotMove = almath.pose2DInverse(
self.init_position) * self.temp_position
if robotMove.theta > 0:
x_update = self.coordinate_init[1] + robotMove.x
y_update = self.coordinate_init[0] + robotMove.y
theta_temp = self.temp_position.theta - self.init_position.theta
self.coordinate_update = [y_update, x_update, theta_temp]
else:
x_update = self.coordinate_init[1] + robotMove.x
y_update = self.coordinate_init[0] - robotMove.y
theta_temp = self.temp_position.theta - self.init_position.theta
self.coordinate_update = [y_update, x_update, theta_temp]
return self.coordinate_update
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Pose Init
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
useSensorValues = False
result = motionProxy.getRobotPosition(useSensorValues)
print "Simple Robot Position", result
# Example showing how to use this information to know the robot's diplacement.
useSensorValues = True
initRobotPosition = almath.Pose2D(
motionProxy.getRobotPosition(useSensorValues))
print "Robot Position", initRobotPosition
# Make the robot move
# motionProxy.moveTo(0.1, 0.0, 0.2)
# 4 foot (4 Ft, 48 in)
# x = 1.2192
# y = 0
# theta = 0
# print x, y, theta
# motionProxy.moveTo(x, y, theta)
# 2 foot (2 ft, 24in)
x = 0.6096
y = 0
theta = 0
print "Move to commands:", x, y, theta
motionProxy.moveTo(x, y, theta)
endRobotPosition = almath.Pose2D(
motionProxy.getRobotPosition(useSensorValues))
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
print "Robot Move:", robotMove
print "End Robot Position:", motionProxy.getRobotPosition(useSensorValues)
# Go to rest position
motionProxy.rest()
def main(session):
"""
This example uses the getNextRobotPosition method.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send NAO to Pose Init
posture_service.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
result = motion_service.getNextRobotPosition()
print "Next Robot Position", result
# Example showing how to use this information to know the robot's diplacement
# during the move process.
# Make the robot move
motion_service.moveTo(0.6, 0.0, 0.5,
_async=True) # No blocking due to post called
time.sleep(1.0)
initRobotPosition = almath.Pose2D(motion_service.getNextRobotPosition())
# Make the robot move
motion_service.moveTo(0.1, 0.0, 0.2)
endRobotPosition = almath.Pose2D(motion_service.getNextRobotPosition())
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
print "Robot Move :", robotMove
# Go to rest position
motion_service.rest()
def walking(motionProxy, X, Y, Theta):
# Enable arms control by move algorithm
motionProxy.setMoveArmsEnabled(True, True)
# FOOT CONTACT PROTECTION
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
# get robot position before move
initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
motionProxy.post.moveTo(X, Y, Theta)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
# get robot position after move
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
# compute and print the robot motion
robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
# return an angle between [-PI, PI]
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
def walking(motionProxy, X, Y, Theta):
# Enable arms control by move algorithm
motionProxy.setMoveArmsEnabled(True, True)
# FOOT CONTACT PROTECTION
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
# get robot position before move
initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
motionProxy.post.moveTo(X, Y, Theta)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
# get robot position after move
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
# compute and print the robot motion
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between [-PI, PI]
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
def main(session):
"""
This example uses the getRobotPosition method.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Pose Init
posture_service.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
useSensorValues = False
result = motion_service.getRobotPosition(useSensorValues)
print "Robot Position", result
# Example showing how to use this information to know the robot's diplacement.
useSensorValues = False
initRobotPosition = almath.Pose2D(
motion_service.getRobotPosition(useSensorValues))
# Make the robot move
motion_service.moveTo(0.1, 0.0, 0.2)
endRobotPosition = almath.Pose2D(
motion_service.getRobotPosition(useSensorValues))
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
print "Robot Move:", robotMove
# Go to rest position
motion_service.rest()
# Second call of move API
motionProxy.post.moveTo(0.5, 0.5, -0.0)
# get the second foot steps vector
footSteps2 = motionProxy.getFootSteps()
# end experiment, begin compute
# here we wait until the move process is over
motionProxy.waitUntilMoveIsFinished()
# then we get the final robot position
robotPositionFinal = almath.Pose2D(motionProxy.getRobotPosition(False))
# compute robot Move with the second call of move API
# so between nextRobotPosition and robotPositionFinal
robotMove = almath.pose2DInverse(nextRobotPosition) * robotPositionFinal
print "Robot Move :", robotMove
# end compute, begin plot
if (HAS_PYLAB):
#################
# Plot the data #
#################
pyl.figure()
printRobotPosition(robotPosition, 'black')
printRobotPosition(nextRobotPosition, 'blue')
printFootSteps(footSteps1, 'green', 'red')
pyl.figure()
printRobotPosition(robotPosition, 'black')
result = motionProxy.getNextRobotPosition()
print "Next Robot Position", result
# Example showing how to use this information to know the robot's diplacement
# during the move process.
# Make the robot move
motionProxy.post.moveTo(0.6, 0.0, 0.5) # No blocking due to post called
time.sleep(1.0)
initRobotPosition = m.Pose2D(motionProxy.getNextRobotPosition())
# Make the robot move
motionProxy.moveTo(0.1, 0.0, 0.2)
endRobotPosition = m.Pose2D(motionProxy.getNextRobotPosition())
# Compute robot's' displacement
robotMove = m.pose2DInverse(initRobotPosition) * endRobotPosition
print "Robot Move :", robotMove
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python almotion_getnextrobotposition.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
def main(session):
"""
Walk To: Small example to make Nao Walk follow a Dubins Curve.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
# first we defined the goal
goal = almath.Pose2D(0.0, -0.4, 0.0)
# We get the dubins solution (control points) by
# calling an almath function
circleRadius = 0.04
# Warning : the circle use by dubins curve
# have to be 4*CircleRadius < norm(goal)
dubinsSolutionAbsolute = almath.getDubinsSolutions(goal, circleRadius)
# moveTo With control Points use relative commands but
# getDubinsSolution return absolute position
# So, we compute dubinsSolution in relative way
dubinsSolutionRelative = []
dubinsSolutionRelative.append(dubinsSolutionAbsolute[0])
for i in range(len(dubinsSolutionAbsolute) - 1):
dubinsSolutionRelative.append(dubinsSolutionAbsolute[i].inverse() *
dubinsSolutionAbsolute[i + 1])
# create a vector of moveTo with dubins Control Points
moveToTargets = []
for i in range(len(dubinsSolutionRelative)):
moveToTargets.append([
dubinsSolutionRelative[i].x, dubinsSolutionRelative[i].y,
dubinsSolutionRelative[i].theta
])
# Initialized the Move process and be sure the robot is ready to move
# without this call, the first getRobotPosition() will not refer to the position
# of the robot before the move process
motion_service.moveInit()
# get robot position before move
robotPositionBeforeCommand = almath.Pose2D(
motion_service.getRobotPosition(False))
motion_service.moveTo(moveToTargets)
# With MoveTo control Points, it's also possible to customize the gait parameters
# motionProxy.moveTo(moveToTargets,
# [["StepHeight", 0.001],
# ["MaxStepX", 0.06],
# ["MaxStepFrequency", 1.0]])
# get robot position after move
robotPositionAfterCommand = almath.Pose2D(
motion_service.getRobotPosition(False))
# compute and print the robot motion
robotMoveCommand = almath.pose2DInverse(
robotPositionBeforeCommand) * robotPositionAfterCommand
print "The Robot Move Command: ", robotMoveCommand
# Go to rest position
motion_service.rest()
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
useSensorValues = False
result = motionProxy.getRobotPosition(useSensorValues)
print "Robot Position", result
# Example showing how to use this information to know the robot's diplacement.
useSensorValues = False
initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
# Make the robot move
motionProxy.moveTo(0.1, 0.0, 0.2)
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
print "Robot Move:", robotMove
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python almotion_getrobotposition.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
# get robot position before move
robotPositionBeforeCommand = m.Pose2D(motionProxy.getRobotPosition(False))
motionProxy.moveTo(moveToTargets)
# With MoveTo control Points, it's also possible to customize the gait parameters
# motionProxy.moveTo(moveToTargets,
# [["StepHeight", 0.001],
# ["MaxStepX", 0.06],
# ["MaxStepFrequency", 1.0]])
# get robot position after move
robotPositionAfterCommand = m.Pose2D(motionProxy.getRobotPosition(False))
# compute and print the robot motion
robotMoveCommand = m.pose2DInverse(
robotPositionBeforeCommand) * robotPositionAfterCommand
print "The Robot Move Command: ", robotMoveCommand
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_moveToDubinsCurve.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
useSensorValues = False
result = motionProxy.getRobotPosition(useSensorValues)
print "Robot Position", result
"""
# Example showing how to use this information to know the robot's diplacement.
useSensorValues = False
initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
# Make the robot move
motionProxy.moveTo(0.1, 0.0, 0.2)
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
"""
# Compute robot's' displacement
robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
print "Robot Move:", robotMove
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python almotion_getrobotposition.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
[dubinRel[i].x,
dubinRel[i].y,
dubinRel[i].theta] )
motionNaoProxy.moveInit()
# get robot position before move
robotNaoPosnBefCommand = m.Pose2D(motionNaoProxy.getRobotPosition(False))
motionNaoProxy.moveTo( moveTarg )
# get robot position after move
robotNaoPosnAftCommand = m.Pose2D(motionNaoProxy.getRobotPosition(False))
# compute and print the robot motion
moveNaoComms = m.pose2DInverse(robotNaoPosnBefCommand)*robotNaoPosnAftCommand
print "The Robot Move Command: ", moveNaoComms
# Go to rest position
motionNaoProxy.rest()
if __name__ == "__main__":
parseFile = argparse.ArgumentParser()
parseFile.add_argument("--ip", type=str, default="127.0.0.1",
help="Robot ip address")
parseFile.add_argument("--port", type=int, default=52742,
help="Robot port number")
args = parseFile.parse_args()
main(args.ip, args.port)
# Second call of move API
motionProxy.post.moveTo(0.3, 0.0, -0.5)
# get the second foot steps vector
footSteps2 = motionProxy.getFootSteps()
# end experiment, begin compute
# here we wait until the move process is over
motionProxy.waitUntilMoveIsFinished()
# then we get the final robot position
robotPositionFinal = almath.Pose2D(motionProxy.getRobotPosition(False))
# compute robot Move with the second call of move API
# so between nextRobotPosition and robotPositionFinal
robotMove = almath.pose2DInverse(nextRobotPosition)*robotPositionFinal
print "Robot Move :", robotMove
# end compute, begin plot
if (HAS_PYLAB):
#################
# Plot the data #
#################
pyl.figure()
printRobotPosition(robotPosition, 'black')
printRobotPosition(nextRobotPosition, 'blue')
printFootSteps(footSteps1, 'green', 'red')
pyl.figure()
printRobotPosition(robotPosition, 'black')
motionProxy.moveInit()
# get robot position before move
robotPositionBeforeCommand = m.Pose2D(motionProxy.getRobotPosition(False))
motionProxy.moveTo( moveToTargets )
# With MoveTo control Points, it's also possible to customize the gait parameters
# motionProxy.moveTo(moveToTargets,
# [["StepHeight", 0.001],
# ["MaxStepX", 0.06],
# ["MaxStepFrequency", 1.0]])
# get robot position after move
robotPositionAfterCommand = m.Pose2D(motionProxy.getRobotPosition(False))
# compute and print the robot motion
robotMoveCommand = m.pose2DInverse(robotPositionBeforeCommand)*robotPositionAfterCommand
print "The Robot Move Command: ", robotMoveCommand
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_moveToDubinsCurve.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Stand Init
postureProxy.goToPosture("StandInit", 0.5)
# first we defined the goal
goal = m.Pose2D(0.0, -0.4, 0.0)
# We get the dubins solution (control points) by
# calling an almath function
circleRadius = 0.04
# Warning : the circle use by dubins curve
# have to be 4*CircleRadius < norm(goal)
dubinsSolutionAbsolute = m.getDubinsSolutions(goal, circleRadius)
# moveTo With control Points use relative commands but
# getDubinsSolution return absolute position
# So, we compute dubinsSolution in relative way
dubinsSolutionRelative = []
dubinsSolutionRelative.append(dubinsSolutionAbsolute[0])
for i in range(len(dubinsSolutionAbsolute)-1):
dubinsSolutionRelative.append(
dubinsSolutionAbsolute[i].inverse() *
dubinsSolutionAbsolute[i+1])
# create a vector of moveTo with dubins Control Points
moveToTargets = []
for i in range(len(dubinsSolutionRelative)):
moveToTargets.append(
[dubinsSolutionRelative[i].x,
dubinsSolutionRelative[i].y,
dubinsSolutionRelative[i].theta] )
# Initialized the Move process and be sure the robot is ready to move
# without this call, the first getRobotPosition() will not refer to the position
# of the robot before the move process
motionProxy.moveInit()
# get robot position before move
robotPositionBeforeCommand = m.Pose2D(motionProxy.getRobotPosition(False))
motionProxy.moveTo( moveToTargets )
# With MoveTo control Points, it's also possible to customize the gait parameters
# motionProxy.moveTo(moveToTargets,
# [["StepHeight", 0.001],
# ["MaxStepX", 0.06],
# ["MaxStepFrequency", 1.0]])
# get robot position after move
robotPositionAfterCommand = m.Pose2D(motionProxy.getRobotPosition(False))
# compute and print the robot motion
robotMoveCommand = m.pose2DInverse(robotPositionBeforeCommand)*robotPositionAfterCommand
print "The Robot Move Command: ", robotMoveCommand
# Go to rest position
motionProxy.rest()
initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
X = 0.3
Y = 0.1
Theta = math.pi/2.0
motionProxy.post.moveTo(X, Y, Theta)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = m.pose2DInverse(initRobotPosition)*endRobotPosition
print "Robot Move :", robotMove
if __name__ == "__main__":
robotIp = "127.0.0.1"
if len(sys.argv) <= 1:
print "Usage python motion_moveTo.py robotIP (optional default: 127.0.0.1)"
else:
robotIp = sys.argv[1]
main(robotIp)
def walk(self):
# x, y, theta = 0
global audio, r
audio = r = None
r = sr.Recognizer()
count = 0
self.motion.wakeUp()
names = "HeadPitch"
angles = -30.0 * m.TO_RAD
fractionMaxSpeed = 0.1
self.motion.setAngles(names, angles, fractionMaxSpeed)
time.sleep(0.1)
# resolution
# kQQVGA (160x120), kQVGA (320x240), kVGA (640x480) or
# k4VGA (1280x960, only with the HD camera).
# color space desired
# kYuvColorSpace, kYUVColorSpace, kYUV422InterlacedColorSpace, kRGBColorSpace, etc.
# frames per second
# Finally, select the minimal number of frames per second (fps) that your
# vision module requires up to 30fps.
name_id = self.video_service.subscribe("walkToH", 2, 11, 20)
print("ALTracker successfully started, now show your face to robot!")
print("Use Ctrl+c to stop this script.")
pixel = 640
try:
while True:
real_focal_length = self.get_focal_length()
if real_focal_length == "no face detected":
self.tts.say("no face detection please try again")
continue
else:
self.tts.say("ok I got your face ")
print("face detected")
break
self.motion.moveInit()
robotPositionBeforeCommand = m.Pose2D(
self.motion.getRobotPosition(False))
while True:
count = count + 1
nao_images = self.video_service.getImageRemote(name_id)
if nao_images is None:
print("Can't capture frames from Nao's camera")
break
cv_images = self.to_cv_img(nao_images)
# time.sleep(0.1)
img = imutils.resize(cv_images,
width=min(pixel, cv_images.shape[1]))
marker = self.find_face(img, face_cascade)
object_marker = self.find_object(img, obj_cascade)
if len(object_marker) is not 1:
# object_marker = [[0, 0, 0, 0]]
print("-------")
print(marker)
x, y, theta = self.foot_step(marker, real_focal_length)
else:
for (xA, yA, xB, yB) in object_marker:
ya_max = yA
yb_max = yB
half_length = (xB - xA) / 2
obj_pix_value = xA + half_length
pix_obj_height = yb_max - ya_max
if pix_obj_height == 0:
return 0, 0, 0
print("-------")
print(object_marker)
inches = self.distance_to_camera(KNOWN_PERSON_HEIGHT,
real_focal_length,
pix_obj_height)
print("object detected", "%.2f inches" % inches)
print("object detected", obj_pix_value)
# print("-------")
if inches > 40.0:
x, y, theta = self.foot_step(marker, real_focal_length)
elif 40 > inches > 0:
if (pixel / 2) - 80 <= obj_pix_value:
x = 0
y = 0.3
theta = 0
elif obj_pix_value < (pixel / 2) + 80:
x = 0
y = -0.3
theta = 0
else:
x, y, theta = self.foot_step(
marker, real_focal_length)
else:
x = 0
y = 0
theta = 0
frequency = 0
self.motion.post.moveTo(x, y, theta, frequency)
self.motion.waitUntilMoveIsFinished()
#self.motion.setWalkTargetVelocity(0.0, 0.0, 0.0, 0.0)
robotPositionAfterCommand = m.Pose2D(
self.motion.getRobotPosition(False))
# compute and print the robot motion
robotMoveCommand = m.pose2DInverse(
robotPositionBeforeCommand) * robotPositionAfterCommand
print("The Robot Move Command: ", robotMoveCommand)
print("-------")
if count > 20:
# print(1)
self.headPitchYaw()
print("I'm tired, can I have a rest?")
self.tts.say("I'm tired \\pau=500\\ can I have a rest ")
self.motion.rest()
# ---------> Recording <---------
self.record.stopMicrophonesRecording()
print('Start recording...')
# tts.say("start recording...")
record_path = speech_record_file_path
self.record.startMicrophonesRecording(
record_path, 'wav', 16000, (0, 0, 1, 0))
time.sleep(3)
self.record.stopMicrophonesRecording()
print('stop recording')
# tts.say("stop recording")
# ---------> Speech recognition <---------
with sr.WavFile("./src/speechRecord.wav") as source:
audio = r.record(source)
text = r.recognize_google(audio)
print(text)
if text == "no":
count = 0
self.motion.wakeUp()
continue
elif text == "yes":
self.tts.say(
"okay \\pau=500\\ I'll have a break\\pau=500\\ touch my front head if you want to play"
)
break
else:
self.tts.say(
"I did not get you\\pau=500\\ I'll have a break ")
break
else:
continue
except SystemExit as e:
print("System error : {0}".format(e))
except OSError as err:
print("OS error: {0}".format(err))
else:
print("Unexpected error:", sys.exc_info()[0])
finally:
print("Unsubscribe the video!")
self.video_service.unsubscribe(name_id)
self.motion.rest()
try:
motionNaoProxy = ALProxy("ALMotion", NaoIP, PORT)
except Exception, e:
print "Error was: ", e
sys.exit(1)
try:
postNaoProxy = ALProxy("ALRobotPosture", NaoIP, PORT)
except Exception, e:
print "Error was: ", e
postNaoProxy.goToPosture("StandInit", 0.5)
useSensValues = False
result = motionNaoProxy.getRobotPosition(useSensValues)
print "Nao Position", result
useSensValues = False
initNaoPosition = almath.Pose2D(
motionNaoProxy.getRobotPosition(useSensValues))
motionNaoProxy.moveTo(41, 1, 0.0)
endNaoPosition = almath.Pose2D(
motionNaoProxy.getRobotPosition(useSensValues))
NaoMove = almath.pose2DInverse(initRobotPosition) * endNaoPosition
print "Nao Move:", NaoMove
if __name__ == "__main__":
NaoIp = "127.0.0.1"
main(NaoIp)
