def test_ops(self):
p0 = almath.Pose2D(0.1, 0.2, 0.2)
p1 = almath.Pose2D(0.3, -0.2, -0.6)
pAdd = p0 + p1
pExp = almath.Pose2D(p0.x + p1.x, p0.y + p1.y, p0.theta + p1.theta)
self.assertTrue(pAdd.isNear(pExp))
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 return_home_with_alnavigation(self):
cpt = 0
if self.slam_pos_in_world:
for i in range(0, 100):
robotPose = almath.Pose2D(
self.services.ALNavigation.getRobotPositionInMap()[0])
center = almath.Pose2D(0.0, 0.0, 0.0)
if robotPose.distance(center) > 0.3:
self.navigation_future = self.services.ALNavigation.navigateToInMap(
[0.0, 0.0],
_async=True)
yield self.navigation_future
if robotPose.distance(center) < 0.3:
poseDiff = robotPose.diff(center)
if math.fabs(poseDiff.theta) > 0.2:
self.navigation_future = self.services.ALMotion.moveTo(
0,
0,
almath.modulo2PI(
poseDiff.theta),
_async=True)
yield self.navigation_future
else:
cpt += 1
if cpt > 3:
self.slam_pos_in_world = [0, 0]
yield stk.coroutines.Return(True)
return
self.logger.info("End back home.")
yield stk.coroutines.Return(False)
def approach_home(self):
x_coord, y_coord, theta = self.services.PositionManager.get_home_pos()
coord_pod = [x_coord, y_coord, theta]
p2D_world2robot = almath.Pose2D(
self.services.ALMotion.getRobotPosition(True))
p2D_world2target = almath.Pose2D(coord_pod)
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
t_world2target = almath.transformFromPose2D(p2D_world2target)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(t_robot2target)
coord_pod = list(p6D_robot2target.toVector())
coord_home = [
coord_pod[0] + DISTANCE_FIRST_APPROACH * math.cos(coord_pod[-1]),
coord_pod[1] + DISTANCE_FIRST_APPROACH * math.sin(coord_pod[-1]),
coord_pod[-1] + math.pi
]
is_success = False
if self.services.PositionManager.turn_toward_position(
[DISTANCE_FIRST_APPROACH, 0]):
x, y, theta = coord_home
r, r_theta = self.get_polar_coord([x, y, theta])
if self.services.ALMotion.moveTo(r, 0, 0, MOVE_CONFIG_HIGH_SPEED):
is_success = True
yield self.services.PositionManager.turn_toward_position(
[0, 0], _async=True)
yield stk.coroutines.Return(is_success)
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 drawRobot(self, robotPose):
# Draw robot triangle
L = self.robotRadius
points = []
frontLeft = self.occupancyMap.getPixelFromPosition2D(
m.position2DFromPose2D(robotPose * m.Pose2D(0.155, 0.1762, 0.0)))
frontRight = self.occupancyMap.getPixelFromPosition2D(
m.position2DFromPose2D(robotPose * m.Pose2D(0.155, -0.1762, 0.0)))
back = self.occupancyMap.getPixelFromPosition2D(
m.position2DFromPose2D(robotPose * m.Pose2D(-0.20, 0.0, 0.0)))
points.append(frontLeft.toVector())
points.append(frontRight.toVector())
points.append(back.toVector())
pygame.draw.lines(self.screen, black, True, points, 8)
pygame.draw.lines(self.screen, robotColor, True, points, 2)
# Front point
frontPoint = self.occupancyMap.getPixelFromPosition2D(
m.position2DFromPose2D(robotPose *
m.Pose2D(0.1, 0.0, 0.0))).toVector()
frontPointInts = [int(frontPoint[0]), int(frontPoint[1])]
pygame.draw.circle(self.screen, black, frontPointInts, 10)
pygame.draw.circle(self.screen, robotColor, frontPointInts, 8)
# Black center dot
robot = self.occupancyMap.getPixelFromPosition2D(
m.position2DFromPose2D(robotPose)).toVector()
robotInts = [int(robot[0]), int(robot[1])]
pygame.draw.circle(self.screen, (0, 0, 0), robotInts, 2)
obstacleList = []
# Target.
target = self.occupancyMap.getPixelFromPosition2D(
m.position2DFromPose2D(self.currentTargetRobotPose2D))
pygame.draw.circle(self.screen, targetColor,
[int(target.x), int(target.y)], 10)
def moverPernas(xMetros, yMetros, eixoGraus, posturaStandInit = True):
try:
if posturaStandInit:
Motor.posturaStandInit()
motion = Motor.movimento()
initPosition = almath.Pose2D(motion.getRobotPosition(True))
targetDistance = almath.Pose2D(xMetros, yMetros, eixoGraus * almath.PI / 180)
expectedEndPosition = initPosition * targetDistance
motion.setMoveArmsEnabled(True, True)
varmovimento = motion.moveTo(xMetros, yMetros, eixoGraus * almath.PI / 180)
positionErrorThresholdPos = 0.01
positionErrorThresholdAng = 0.03
# The move is finished so output
realEndPosition = almath.Pose2D(motion.getRobotPosition(False))
positionError = realEndPosition.diff(expectedEndPosition)
positionError.theta = almath.modulo2PI(positionError.theta)
if (abs(positionError.x) < positionErrorThresholdPos
and abs(positionError.y) < positionErrorThresholdPos
and abs(positionError.theta) < positionErrorThresholdAng):
print("Movimento OK")
else:
print("Movimento ERROR", positionError.toVector())
Motor.posturaCrouch()
ModoAutonomo.on()
time.sleep(0.25)
ModoAutonomo.off()
time.sleep(0.25)
Motor.posturaStandInit()
return varmovimento
except Exception as e:
print("Exception -> Motor.moverPernas():", e)
return False
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 getPositionDiff(self, target):
robotPose = self.navigation.getRobotPositionInMap()
currentPose = almath.Pose2D(robotPose[0][0], robotPose[0][1],
robotPose[0][2])
targetPose = almath.Pose2D(target)
poseDiff = currentPose.inverse() * targetPose
positionDiff = almath.position2DFromPose2D(poseDiff)
#angle = positionDiff.getAngle()
return positionDiff
def find_home(self, research_360_activated=False):
"returns (coords) of any code it finds, or (None)."
distance_pod = None
mode_search = False
if not self.services.PositionManager:
yield stk.coroutines.Return(None)
if self.pod_pos_in_world and not research_360_activated:
pod_future = self.services.PositionManager.turn_toward_position(
[-1.0, 0], _async=True)
yield pod_future
distance_pod = self.services.PositionManager.get_dist_from_robot(
-0.70, 0)
pod_future = self.services.ALTracker._lookAtWithEffector(
[distance_pod, 0, 0], 2, 2, 0.1, 0, _async=True)
yield pod_future
else:
mode_search = True
pod_future = None
if mode_search:
pod_future = self.search_home()
yield pod_future
self.logger.info(repr(pod_future.value()))
else:
yield self.services.ALRecharge.setUseTrackerSearcher(False,
_async=True)
pod_future = qi. async (self.services.ALRecharge.lookForStation)
self.future_sleep = stk.coroutines.sleep(5)
yield self.future_sleep
if pod_future.isRunning():
self.services.ALRecharge.stopAll()
yield stk.coroutines.Return(None)
if pod_future:
coord_pod = pod_future.value()
if coord_pod and coord_pod[1] and len(coord_pod[1]) == 3:
p2D_world2robot = almath.Pose2D(
self.services.ALMotion.getRobotPosition(True))
coord_pod = coord_pod[1]
self.services.PositionManager.init_position_with_coord(
coord_pod)
self.pod_pos_in_world = [0.0, 0.0]
p2D_world2target = almath.Pose2D(coord_pod)
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
t_world2target = almath.transformFromPose2D(p2D_world2target)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(
t_robot2target)
coord_pod = list(p6D_robot2target.toVector())
coord_home = [
coord_pod[0] + DISTANCE_FROM_POD * math.cos(coord_pod[-1]),
coord_pod[1] + DISTANCE_FROM_POD * math.sin(coord_pod[-1]),
coord_pod[-1]
]
yield stk.coroutines.Return(coord_home)
yield stk.coroutines.Return(None)
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 callback(self, poseStamped):
pose = Pose(poseStamped.pose.position, poseStamped.pose.orientation)
quat = almath.Quaternion(pose.orientation.w, pose.orientation.x,
pose.orientation.y, pose.orientation.z)
rotation = almath.rotation3DFromQuaternion(quat)
position3d = almath.Position3D(pose.position.x, pose.position.y,
pose.position.z)
worldToTarget = almath.Pose2D(position3d.x, position3d.y, rotation.wz)
worldToRobot = almath.Pose2D(self.motionProxy.getRobotPosition(True))
robotToTarget = almath.pinv(worldToRobot) * worldToTarget
robotToTarget.theta = almath.modulo2PI(robotToTarget.theta)
self.motionProxy.moveTo(robotToTarget.x, robotToTarget.y,
robotToTarget.theta)
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 drawLocalizationResult(self):
if len(self.localizationResult) != 2:
return
incert = m.Pose2D(self.localizationResult[1])
ellipseRadius = math.sqrt(incert.x * incert.x + incert.y * incert.y)
r = int(ellipseRadius / self.occupancyMap.mpp)
# Do not draw if radius circle is less than 3 pixels.
if r > 3:
pose = m.Pose2D(self.localizationResult[0])
p = self.occupancyMap.getPixelFromPosition2D(
m.Position2D(pose.x, pose.y)).toVector()
pInts = (int(p[0]), int(p[1]))
zoneColor = [100, 100, 200]
pygame.draw.circle(self.screen, zoneColor, pInts, r, 3)
def get_home_position(self, params):
p6Ds = self.detect_with_try_params_list(params)
p6Ds_list = {}
params_list = []
if not isinstance(params, list):
params_list.append(dict(DEFAULT_PARAMS))
else:
params_list = params
id_list = []
for tmp_params in params_list:
id_list += tmp_params["ids"]
for ids in id_list:
try:
p6D_world2target = almath.Position6D(p6Ds[ids]["world2target"])
t_world2target = almath.transformFromPosition6D(
p6D_world2target)
self.logger.info("@@@ TARGET IN WORLD %s" % p6D_world2target)
p2D_world2robot = almath.Pose2D(
self.ALMotion.getRobotPosition(True))
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
self.logger.info("@@@ ROBOT POSITION %s" % p2D_world2robot)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(
t_robot2target)
self.logger.info("@@@ TARGET IN ROBOT %s" % p6D_robot2target)
p6Ds_list[ids] = list(p6D_robot2target.toVector())
except Exception as e:
pass
return p6Ds_list
def convert_laser_position(self, sensor_device, laser_x, laser_y):
t_world2robot = almath.transformFromPose2D(almath.Pose2D(self.robot_pose))
t_device2point = almath.transformFromPosition3D(almath.Position3D(laser_x, laser_y, 0))
t_robot2device = almath.transformFromPosition6D(self.device_position_list[sensor_device])
t_world2point = t_world2robot * t_robot2device * t_device2point
p2d_world2point = almath.pose2DFromTransform(t_world2point)
return (p2d_world2point.x, p2d_world2point.y)
def run(self):
color = ["red", "blue", "green"]
ax = plt.axes()
try:
while True:
ax.cla()
ax.set_xlim(-8,8)
ax.set_ylim(-8,8)
# draw robot
robot_pose = almath.Pose2D(self.robot_pose)
xn = robot_pose.x + 0.2 * math.cos(robot_pose.theta)
yn = robot_pose.y + 0.2 * math.sin(robot_pose.theta)
plt.plot([robot_pose.x, xn], [robot_pose.y, yn], c="black")
c = patches.Circle(xy=(robot_pose.x, robot_pose.y), radius=0.2, fill=False, color="black")
ax.add_patch(c)
#laser_values = self.convert_laser_position(self.get_laser_values())
for sensor_device in range(1):
print self.position[sensor_device]
for value in range(15):
laser = self.position[sensor_device][value]
try:
plt.scatter(laser[0], laser[1], s=20)
except Exception as errorMsg1:
failure(errorMsg1)
ax.set_aspect("equal")
plt.pause(0.02)
except Exception as errorMsg:
failure("running is failed: {}".format(str(errorMsg)))
exit(-1)
def leftHand_process(self):
#plik = open('./plik.txt','w')
#count_test = 500
sequence = 0
count = 0
#while count_test > 0:
while not rospy.is_shutdown():
trans_rh, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_r_hand, rospy.Duration())
trans_erh, rot = self.tf.lookupTransform('/openni_depth_frame', self.j_r_ehand, rospy.Duration())
vec_rh = Vector3(*trans_rh)
vec_reh = Vector3(*trans_erh)
#pozycja inicjujaca. reka ulozona w litere L
if ((vec_rh.z > ( vec_reh.z + 0.25)) and (vec_rh.y < vec_reh.y + 0.1) and (vec_rh.y > vec_reh.y - 0.1) and (sequence == 0 or sequence == 4)):
print('Start')
sequence += 1
if (count == 1):
sequence = 1
if ( (sequence == 1 or sequence==3) and (vec_rh.y > vec_reh.y - 0.25) and (vec_rh.y < vec_reh.y - 0.15) and (vec_rh.z > ( vec_reh.z + 0.15))):
print('1')
sequence += 1
if ( sequence == 2 and (vec_rh.y < vec_reh.y - 0.25) and (vec_rh.z < ( vec_reh.z + 0.1))):
print('2')
sequence += 1
count += 1
if (count == 2):
print('go to LEFT!')
gotoPosition = almath.Pose2D(self.motionProxy.getRobotPosition(False))
gotoPosition = gotoPosition-self.mistake
self.nao_go(gotoPosition.x,gotoPosition.y - 0.5)
#self.nao_go(self.naoXPosition,self.naoYPosition - 0.5)
count =0
sequence = 0
#plik.write(str(vec_rh.x) + " " + str(vec_rh.y) + " " + str(vec_rh.z) + " " + str(vec_reh.x) + " " + str(vec_reh.y) + " " + str(vec_reh.z)+ "\n")
#count_test -=1
self.r.sleep()
def main(session):
"""
This example alocate pepper in world coordinate
"""
# Get the services ALNavigation and ALMotion.
navigation_service = session.service("ALNavigation")
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)
#print "Standing"
# moveAlong
time.sleep(1)
#result = navigation_service.moveAlong(["Composed", ["Holonomic", ["Line", [2.0, 0.0]], 0.0, 5.0], ["Holonomic", ["Line", [1, 0.0]], 0.0, 10.0]])
#print "Navigation result", result
# moveAlong Circle
time.sleep(1)
result = navigation_service.moveAlong(
["Holonomic", ["Circle", [2.0, 2.0], 2.0], 2.0, 5.0])
print "Navigation result", result
# find robot position
pose = almath.Pose2D(motion_service.getRobotPosition(False))
print "Pose", pose.x, pose.y, pose.theta
def get_odometric_position_error(self):
cumulated_displacement = almath.Pose2D(
self.services.ALMotion._getCumulatedDisplacement())
last_cumulated_displasment = cumulated_displacement - self.relative_cumulated_displacement
error_xy = last_cumulated_displasment.norm() * 0.03
error_theta = last_cumulated_displasment.theta * 0.01
return [error_xy, error_theta]
def move_routine(self):
robot_position = almath.Pose2D(self.ALMotion.getRobotPosition(True))
if self.robot_position_begin:
distance_moved = self.robot_position_begin.distance(robot_position)
self.cpt = 0
if distance_moved >= self.distance_to_do:
self.mode = "end_move"
self._stop()
elif (distance_moved <=
(self.distance_to_do -
self.slow_down_distance)) and self.mode != "low_speed":
if self.mode == "full_speed":
self.future = self.ALMotion.move(
CONFIGURATIONS[self.mode]["vcc"], 0, 0, _async=True)
self.mode = "full_speed_started"
else:
if self.mode == "full_speed_started":
self.mode = "low_speed"
if self.mode == "low_speed":
self.future = self.ALMotion.move(
CONFIGURATIONS[self.mode]["vcc"], 0, 0, _async=True)
self.mode = "low_speed_started"
if self.last_distance == distance_moved:
self.cpt += 1
if self.cpt >= 100:
self.reason = "can't move during 1 seconds"
self._stop()
else:
self.last_distance = distance_moved
def __init__(self, qiapp):
self.qiapp = qiapp
self.session = qiapp.session
self.events = stk.events.EventHelper(self.session)
self.services = stk.services.ServiceCache(self.session)
self.logger = stk.logging.get_logger(self.session, self.APP_ID)
# Wait some services
self.session.waitForService("ALMotion")
self.session.waitForService("ALMemory")
self.session.waitForService("ALPreferenceManager")
# Position of the robot initial
xw0, yw0, thetaw0 = self.services.ALMotion.getRobotPosition(True)
self.home_frame = pu.Pose(x_coord=xw0, y_coord=yw0, theta=thetaw0)
self.robot_in_home = self.home_frame.get_relative(self.home_frame)
# Load saved position
if os.path.exists(PATH_POSITION_FILE):
self.__load_position_in_file()
else:
folder = os.path.dirname(PATH_POSITION_FILE)
if not os.path.exists(folder):
os.makedirs(folder)
self.__save_position_in_file()
# Save the time for checking if we need to relocalize or not
# We need to relocalize each X minutes for safety and being
# sur to know where we are.
self.last_time_reloc = time.time()
# periodic task
self.relative_cumulated_displacement = almath.Pose2D(
self.services.ALMotion._getCumulatedDisplacement())
def interpretJointsPose(motionProxy, memoryProxy):
''' Translates the current left arm pose into a target position for NAO's
foot. '''
# Retrieve current arm position.
armPose = motionProxy.getAngles(armName, True)
targetX = 0.0
targetY = 0.0
targetTheta = 0.0
gaitConfig = motionProxy.getMoveConfig("Default")
# Filter Shoulder Pitch.
if (armPose[0] > -0.9 and armPose[0] < -0.20):
targetX = stepLength
elif (armPose[0] > -2.5 and armPose[0] < -1.5):
targetX = -stepLength - 0.02
# Filter Wrist Yaw.
if (armPose[4] > 0.2):
targetTheta = gaitConfig[2][1]
elif (armPose[4] < -0.2):
targetTheta = -gaitConfig[2][1]
# Return corresponding pose.
return almath.Pose2D(targetX, targetY, targetTheta)
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 printRightFootStep(footPose, color, size):
""" Function for plotting a RIGHT foot step
:param footPose: an almath Pose2D
:param color: the color for the foot step
:param size: the size of the line
"""
rFootBoxFL = footPose * almath.Pose2D( 0.110, 0.038, 0.0)
rFootBoxFR = footPose * almath.Pose2D( 0.110, -0.050, 0.0)
rFootBoxRR = footPose * almath.Pose2D(-0.047, -0.050, 0.0)
rFootBoxRL = footPose * almath.Pose2D(-0.047, 0.038, 0.0)
pyl.plot(footPose.x, footPose.y, color=color, marker='o', markersize=size*2)
pyl.plot( [rFootBoxFL.x, rFootBoxFR.x, rFootBoxRR.x, rFootBoxRL.x, rFootBoxFL.x],
[rFootBoxFL.y, rFootBoxFR.y, rFootBoxRR.y, rFootBoxRL.y, rFootBoxFL.y],
color = color,
linewidth = size)
def __init__(self, ip):
self.IP = ip
self.running = True
self.occupancyMap = OccupancyMap(700, 0.01, m.Position2D(0, 0))
self.robotPose = m.Pose2D(0, 0, 0)
self.robotRadius = 0.3 # meters
self.nodeNumber = 0
self.map = []
self.path = []
self.localizationResult = []
self.downSelectedPositionWorld = m.Position2D()
self.currentTargetRobotPose2D = m.Pose2D(200, 200, 0)
self.downScrollPosition = m.Position2D()
self.lastUpdateScrollPosition = m.Position2D(10000000, 10000000)
#init
self.connectionToRobot()
self.initPygame()
def printFootSteps(footSteps, colorLeft, colorRight):
""" Function for plotting the result of a getFootSteps
:param footSteps: the result of a getFootSteps API call
:param colorLeft: the color for left foot steps
:param colorRight: the color for right foot steps
"""
if (len(footSteps[0]) == 2):
posLeft = footSteps[0][0]
posRight = footSteps[0][1]
if (posLeft != posRight):
leftPose2D = almath.Pose2D(posLeft[0], posLeft[1], posLeft[2])
printLeftFootStep(leftPose2D, colorLeft, 3)
rightPose2D = almath.Pose2D(posRight[0], posRight[1], posRight[2])
printRightFootStep(rightPose2D, colorRight, 3)
if (len(footSteps[1]) >= 1):
for i in range(len(footSteps[1])):
name = footSteps[1][i][0]
pos = footSteps[1][i][2]
tmpPose2D = almath.Pose2D(pos[0], pos[1], pos[2])
if (name == 'LLeg'):
leftPose2D = rightPose2D * tmpPose2D
printLeftFootStep(leftPose2D, colorLeft, 3)
else:
rightPose2D = leftPose2D * tmpPose2D
printRightFootStep(rightPose2D, colorRight, 3)
if (len(footSteps[2]) >= 1):
for i in range(len(footSteps[2])):
name = footSteps[2][i][0]
pos = footSteps[2][i][2]
tmpPose2D = almath.Pose2D(pos[0], pos[1], pos[2])
if (name == 'LLeg'):
leftPose2D = rightPose2D * tmpPose2D
printLeftFootStep(leftPose2D, colorLeft, 1)
else:
rightPose2D = leftPose2D * tmpPose2D
printRightFootStep(rightPose2D, colorRight, 1)
pyl.axis('equal')
def draw_robot(self, ax):
robot_pose = almath.Pose2D(self.robot_pose)
xn = robot_pose.x + 0.2 * math.cos(robot_pose.theta)
yn = robot_pose.y + 0.2 * math.sin(robot_pose.theta)
plt.plot([robot_pose.x, xn], [robot_pose.y, yn], c="black")
c = patches.Circle(xy=(robot_pose.x, robot_pose.y),
radius=0.2,
fill=False,
color="black")
ax.add_patch(c)
def _is_target_reached(self):
self.logger.info("_is_target_reached")
robot_position = almath.Pose2D(self.ALMotion.getRobotPosition(True))
if self.robot_position_begin:
distance_moved = self.robot_position_begin.distance(robot_position)
if abs(distance_moved - self.distance_to_do) < self.error_odom:
self.logger.info("target reached")
return True
self.logger.info("target not reached")
return False
