def kick(self, motion_proxy):
frame = mot.FRAME_TORSO
axis_mask = almath.AXIS_MASK_ALL # full control
use_sensor_values = False
self.shift_weight(use_sensor_values, motion_proxy)
# move LLeg back
effector = "LLeg"
times = 4.0 # seconds
current_pos = motion_proxy.getPosition(effector, frame,
use_sensor_values)
target_pos = almath.Position6D(current_pos)
target_pos.x -= 0.1
target_pos.wy -= 0.03
path_list = [list(target_pos.toVector())]
motion_proxy.positionInterpolation(effector, frame, path_list,
axis_mask, times, True)
# swing LLeg forward
times = [0.2, 0.3] # seconds
current_pos = motion_proxy.getPosition(effector, frame,
use_sensor_values)
target_pos = almath.Position6D(current_pos)
target_pos.x += 0.15
target_pos.wy -= 0.03
path_list = [list(target_pos.toVector())]
target_pos = almath.Position6D(current_pos)
target_pos.x += 0.24
target_pos.wy -= 0.03
path_list.append(list(target_pos.toVector()))
motion_proxy.positionInterpolation(effector, frame, path_list,
axis_mask, times, True)
def keyboardControl(self, command):
if command == "Up":
# Move up 0.01 meter
currentPos = self.motionProxy.getPosition("RArm",
motion.FRAME_TORSO,
False)
targetPos = almath.Position6D(currentPos)
targetPos.x = targetPos.x + 0.01 # meter
self.motionProxy.positionInterpolations("RArm", motion.FRAME_TORSO,
list(targetPos.toVector()),
motion.AXIS_MASK_VEL, 2.0)
self.adjust()
if command == "Down":
# Move down 0.01 meter
currentPos = self.motionProxy.getPosition("RArm",
motion.FRAME_TORSO,
False)
targetPos = almath.Position6D(currentPos)
targetPos.x = targetPos.x - 0.01 # meter
self.motionProxy.positionInterpolations("RArm", motion.FRAME_TORSO,
list(targetPos.toVector()),
motion.AXIS_MASK_VEL, 2.0)
self.adjust()
if command == "Left":
# Move left 0.01 meter
currentPos = self.motionProxy.getPosition("RArm",
motion.FRAME_TORSO,
False)
targetPos = almath.Position6D(currentPos)
targetPos.y = targetPos.y + 0.01 # meter
self.motionProxy.positionInterpolations("RArm", motion.FRAME_TORSO,
list(targetPos.toVector()),
motion.AXIS_MASK_VEL, 2.0)
self.adjust()
if command == "Right":
# Move right 0.01 meter
currentPos = self.motionProxy.getPosition("RArm",
motion.FRAME_TORSO,
False)
targetPos = almath.Position6D(currentPos)
targetPos.y = targetPos.y - 0.01 # meter
self.motionProxy.positionInterpolations("RArm", motion.FRAME_TORSO,
list(targetPos.toVector()),
motion.AXIS_MASK_VEL, 2.0)
self.adjust()
if command == "PenUp":
self.penUp()
if command == "PenDown":
self.penDown()
def test_ops(self):
p0 = almath.Position6D(0.1, 0.2, 0.4, -0.3, 0.8, 12.0)
p1 = almath.Position6D(0.3, -0.2, 0.9, -0.1, 0.4, 2.0)
pAdd = p0 + p1
pExp = almath.Position6D(p0.x + p1.x, p0.y + p1.y, p0.z + p1.z,
p0.wx + p1.wx, p0.wy + p1.wy, p0.wz + p1.wz)
self.assertTrue(pAdd.isNear(pExp))
factor = 0.2
pFact = p0 * factor
pExp = almath.Position6D(p0.x * factor, p0.y * factor, p0.z * factor,
p0.wx * factor, p0.wy * factor,
p0.wz * factor)
self.assertTrue(pFact.isNear(pExp))
def get_closest_landmark_non_move(self, robot_pose, landmark):
side_range = [-math.radians(70), math.radians(70)]
min_distance = 100
min_landmark_id = -1
for id in landmark:
# calculate angle robot to landmark
# マップ原点座標から見たランドマークの位置・姿勢
map2landmark = almath.Position6D(landmark[id])
t_map2landmark = almath.transformFromPosition6D(map2landmark)
# map原点座標から見たロボットの位置・姿勢
map2robot = almath.position6DFromPose2D(robot_pose)
t_map2robot = almath.transformFromPosition6D(map2robot)
# ロボットから見たランドマークの位置・姿勢
t_robot2landmark = t_map2robot.inverse() * t_map2landmark
robot2landmark = almath.position6DFromTransform(t_robot2landmark)
#print(id, robot2landmark, robot2landmark.norm())
local_angle_xy, local_angle_xz = self._get_angle_vector(
robot2landmark)
if (side_range[0] < local_angle_xy < side_range[1]) and (
side_range[0] < local_angle_xz < side_range[1]):
if robot2landmark.norm() < min_distance:
min_distance = robot2landmark.norm()
min_landmark_id = id
#print(math.degrees(local_angle_xy))
return min_landmark_id
def get_closest_landmark(self, robot_pose, landmark):
distance_info = dict()
side_range = [-math.radians(70), math.radians(70)]
for id in landmark:
# calculate angle robot to landmark
# マップ原点座標から見たランドマークの位置・姿勢
map2landmark = almath.Position6D(landmark[id])
t_map2landmark = almath.transformFromPosition6D(map2landmark)
# map原点座標から見たロボットの位置・姿勢
map2robot = almath.position6DFromPose2D(robot_pose)
t_map2robot = almath.transformFromPosition6D(map2robot)
# ロボットから見たランドマークの位置・姿勢
t_robot2landmark = t_map2robot.inverse() * t_map2landmark
robot2landmark = almath.position6DFromTransform(t_robot2landmark)
local_angle_xy, local_angle_xz = self._get_angle_vector(
robot2landmark)
# 1. xz面において任意の角度内に収まっている
# TODO この時点でnullが帰ってくる可能性がある
print(math.degrees(local_angle_xz))
#if (side_range[0] < local_angle_xz < side_range[1]):
# ベクトルのノルムを格納する
distance_info[id] = local_angle_xy #robot2landmark.norm()
# 2. xy面においてベクトル同士のなす角がすべてのランドマークに対して最小か
# 1.でフィルタしたランドマークリストを元に角度が小さい順にソートする
#min_k = min(d, key=d.get)
#min_landmark_id = min(distance_info, key=distance_info.get)
distance_info = sorted(distance_info.items(), key=lambda x: x[1])
print(distance_info)
return distance_info
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 test_init(self):
p = almath.Position6D()
self.assertAlmostEqual(p.x, 0.0)
self.assertAlmostEqual(p.y, 0.0)
self.assertAlmostEqual(p.z, 0.0)
self.assertAlmostEqual(p.wx, 0.0)
self.assertAlmostEqual(p.wy, 0.0)
self.assertAlmostEqual(p.wz, 0.0)
def keyboardControl(self):
self.count = 0
app = drawAction.drawAction()
self.screen.fill((255, 255, 255))
while True:
currentPos = app.motionProxy.getPosition("RHand", motion.FRAME_TORSO, False)
currentPos = almath.Position6D(currentPos)
# Change coordiante from torso to right shoulder joint
self.printText("Press 'Esc' to exit.", "Times New Roman", 20, 10, 10, (255, 0, 0))
self.printText("X Coordinate: " + str(round(currentPos.x, 3)), "Times New Roman", 15, 10, 40, (255, 0, 0))
self.printText("Y Coordinate: " + str(round(currentPos.y + 0.098, 3)), "Times New Roman", 15, 10, 65,(255, 0, 0))
self.printText("Z Coordinate: " + str(round(currentPos.z - 0.1, 3)), "Times New Roman", 15, 10, 90,(255, 0, 0))
# max fps limit
self.clock.tick(30)
for event in pygame.event.get():
if event.type == pygame.QUIT:
return
elif event.type == pygame.KEYDOWN:
# press esc to clear screen
if event.key == pygame.K_ESCAPE:
pygame.display.quit()
# pygame.quit()
return
# press space to get screenshot then clear the screen
if event.key == pygame.K_SPACE:
self.count = self.count + 1
pygame.image.save(self.screen,"keyboard"+str(self.count)+".jpg")
self.screen.fill((255, 255, 255))
if event.key == pygame.K_w:
# Go Up
app.keyboardControl("Up")
if event.key == pygame.K_s:
# Go Down
app.keyboardControl("Down")
if event.key == pygame.K_a:
# Go Left
app.keyboardControl("Left")
if event.key == pygame.K_d:
# Go Right
app.keyboardControl("Right")
if event.key == pygame.K_UP:
# Pen up
app.keyboardControl("PenUp")
if event.key == pygame.K_DOWN:
# Pen down
app.keyboardControl("PenDown")
elif event.type == pygame.MOUSEBUTTONDOWN:
#print "Down"
self.brush.start_draw(event.pos)
elif event.type == pygame.MOUSEMOTION:
if self.brush.drawing:
#print "Draw"
self.brush.draw(event.pos)
elif event.type == pygame.MOUSEBUTTONUP:
#print "Up"
self.brush.end_draw()
pygame.draw.rect(self.screen, (255, 255, 255), (0, 0, 200, 120))
pygame.display.update()
def RoteElCuerpo(Theta, AnguloHeadYaw, AnguloHeadPitch):
fractionMaxSpeed = 0.5
axisMask = 7 # just control position
motionProxy.moveTo(0.0, 0.0, Theta)
angulos = [0, 0]
nombres = ["HeadYaw", "HeadPitch"]
motionProxy.setAngles(nombres, angulos, fractionMaxSpeed)
theHeadPosition = motionProxy.getPosition("Head", 0, False)
targetPos = almath.Position6D(theHeadPosition)
def init_config(self):
self.device_position_list = list()
device_list = ["LaserSensor/Front", "LaserSensor/Left", "LaserSensor/Right"]
for device in device_list:
self.device_position_list.append(almath.Position6D(self.motion.getPosition(device, 2, 0)))
self.robot_pose = self.motion.getRobotPosition(True)
self.robot_moving = self.monitor.isMoving.value()
self.move_signal_link = self.monitor.isMoving.connect(self.on_robot_moving)
self.laser_callback.start(True)
def test_distance(self):
p0 = almath.Position6D()
self.assertAlmostEqual(p0.distance(p0), 0.0)
p1 = almath.Position6D(0.2, 0.0, 0.0, 0.0, 0.0, 0.0)
self.assertAlmostEqual(p0.distance(p1), p1.x)
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 use positionInterpolations
frame = motion.FRAME_ROBOT
useSensorValues = False
dx = 0.03 # translation axis X (meters)
dy = 0.04 # translation axis Y (meters)
# Motion of Arms with block process
effectorList = []
pathList = []
axisMaskList = [motion.AXIS_MASK_VEL, motion.AXIS_MASK_VEL]
timeList = [[1.0], [1.0]] # seconds
effectorList.append("LArm")
currentPos = motionProxy.getPosition("LArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.y -= dy
pathList.append(list(targetPos.toVector()))
effectorList.append("RArm")
currentPos = motionProxy.getPosition("RArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.y += dy
pathList.append(list(targetPos.toVector()))
motionProxy.positionInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
# Motion of Arms and Torso with block process
axisMaskList = [motion.AXIS_MASK_VEL,
motion.AXIS_MASK_VEL,
motion.AXIS_MASK_ALL]
timeList = [[4.0],
[4.0],
[1.0, 2.0, 3.0, 4.0]] # seconds
effectorList = []
pathList = []
effectorList.append("LArm")
pathList.append([motionProxy.getPosition("LArm", frame, useSensorValues)])
effectorList.append("RArm")
pathList.append([motionProxy.getPosition("RArm", frame, useSensorValues)])
effectorList.append("Torso")
torsoList = []
currentPos = motionProxy.getPosition("Torso", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.y += dy
torsoList.append(list(targetPos.toVector()))
targetPos = almath.Position6D(currentPos)
targetPos.x -= dx
torsoList.append(list(targetPos.toVector()))
targetPos = almath.Position6D(currentPos)
targetPos.y -= dy
torsoList.append(list(targetPos.toVector()))
targetPos = almath.Position6D(currentPos)
torsoList.append(list(targetPos.toVector()))
pathList.append(torsoList)
motionProxy.positionInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
# Go to rest position
motionProxy.rest()
print '' print 'Check Position2D' position2D = m.Position2D(0.1, 0.2) print position2D.__repr__() print position2D print '' print 'Check Position3D' position3D = m.Position3D(0.1, 0.2, 0.3) print position3D.__repr__() print position3D print '' print 'Check Position6D' position6D = m.Position6D(0.1, 0.2, 0.3, 0.4, 0.5, 0.6) print position6D.__repr__() print position6D print '' print 'Check PositionAndVelocity' PositionAndVelocity = m.PositionAndVelocity(0.1, 0.2) print PositionAndVelocity.__repr__() print PositionAndVelocity print '' print 'Check Rotation' Rotation = m.Rotation.fromRotX(0.1) print Rotation.__repr__() print Rotation print ''
def ball_location(req):
'''
The service receives the x,y,z coordinates of the object it has to grab.
The grabbing process is done through a series of steps, provided the object is in front of the robot.
'''
posX=req.pos_ini[0]
posY=req.pos_ini[1]
posZ=req.pos_ini[2]
effectorList = []
pathList = []
#########################################################################################################
# Step 1: Open arms wide so you don't hit the box.
# Since a big box is the platform, NAO first opens his arms wide to avoid getting stuck with it.
#########################################################################################################
Larm = "LArm"
Rarm = "RArm"
frame = motion.FRAME_TORSO
useSensorValues = False
fractionMaxSpeed = 0.01
axisMask = 7
axisMaskList = [motion.AXIS_MASK_VEL, motion.AXIS_MASK_VEL]
timeList = [[2.0], [2.0]]
target_larm = [0.05, 0.25, 0, 0, 0, 0]
target_rarm = [0.05, - 0.25, 0, 0, 0, 0]
effectorList.append("LArm")
targetPos = almath.Position6D(target_larm)
pathList.append(list(targetPos.toVector()))
effectorList.append("RArm")
targetPos = almath.Position6D(target_rarm)
pathList.append(list(targetPos.toVector()))
motionProxy.positionInterpolations(effectorList, frame, pathList, axisMaskList, timeList)
time.sleep(2)
#########################################################################################################
# Step 2: Put LArm and RArm effectors on both sides of the ball.
# Initial movement, to center the target slowly and put it between hands, in case the robot
# is not exactly in front of the target.
# A loop is started here, since this process is going to be repeated up to 5 times, in case the object was
# not grabbed. If the object is not grabbed after three tries, it is abandoned, since it most likely fell from
# the platform.
#########################################################################################################
for i in range(5):
effectorList = []
pathList = []
Larm = "LArm"
Rarm = "RArm"
frame = motion.FRAME_TORSO
useSensorValues = False
fractionMaxSpeed = 0.01
axisMask = 7
axisMaskList = [motion.AXIS_MASK_VEL, motion.AXIS_MASK_VEL]
timeList = [[3.0], [3.0]]
target_larm = [posX, posY + 0.15, posZ, 0, 0, 0]
target_rarm = [posX, posY - 0.15, posZ, 0, 0, 0]
effectorList.append("LArm")
targetPos = almath.Position6D(target_larm)
pathList.append(list(targetPos.toVector()))
effectorList.append("RArm")
targetPos = almath.Position6D(target_rarm)
pathList.append(list(targetPos.toVector()))
motionProxy.positionInterpolations(effectorList, frame, pathList, axisMaskList, timeList)
time.sleep(3)
#########################################################################################################
# 3 Step 3: Turn wrists in a proper grasping angle.
#########################################################################################################
left_ang_wrist_rad= -10
right_ang_wrist_rad= 10
#names = ["LWristYaw", "RWristYaw", "LElbowYaw", "RElbowYaw"]
names = ["LWristYaw", "RWristYaw"]
#angleLists = [left_ang_wrist_rad*almath.TO_RAD, right_ang_wrist_rad*almath.TO_RAD, left_ang_elbow_rad*almath.TO_RAD, right_ang_elbow_rad*almath.TO_RAD]
angleLists = [left_ang_wrist_rad*almath.TO_RAD, right_ang_wrist_rad*almath.TO_RAD]
#timeLists = [2.0, 2.0, 2.0, 2.0]
timeLists = [1.0, 1.0]
isAbsolute = True
motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute)
time.sleep(1)
#########################################################################################################
# 4 Now open your hands...
#########################################################################################################
motionProxy.openHand("LHand")
motionProxy.openHand("RHand")
#########################################################################################################
# 5 Step 5 NAO squeezes the target and closes his right hand. And keeps it closed.
#########################################################################################################
fractionMaxSpeed = 0.1
effectorList = []
pathList = []
dy = 0.11
timeLists = [2.0, 2.0]
effectorList.append("LArm")
currentPos = motionProxy.getPosition("LArm", frame, useSensorValues)
targetPos = almath.Position6D(target_larm)
targetPos.y -= dy
pathList.append(list(targetPos.toVector()))
effectorList.append("RArm")
currentPos = motionProxy.getPosition("RArm", frame, useSensorValues)
targetPos = almath.Position6D(target_rarm)
targetPos.y += dy
pathList.append(list(targetPos.toVector()))
motionProxy.positionInterpolations(effectorList, frame, pathList, axisMaskList, timeList)
time.sleep(2)
#########################################################################################################
# 6 Close your hand...
#########################################################################################################
for x in range(0 ,50):
motionProxy.setStiffnesses("RHand", 1.0)
motionProxy.post.setAngles("RHand", 0.0, 0.1)
time.sleep(2)
#########################################################################################################
# 7 Check if hand is closed
#########################################################################################################
sensedAngles = motionProxy.getAngles("RHand", True) #Checks the sensed angles.
if sensedAngles > 0.1:
print ("Ball grabbed successfully.")
break
else:
print ("Ball grabbed UNsuccessfully")
#########################################################################################################
# Step 8: Now with the object in hand, NAO slowly attempts to put his arms to the side of his body
# slowly, without dropping the object or hitting the box. After that, control is returned to the planner.
#########################################################################################################
effectorList = []
pathList = []
timeList = [[4.0], [4.0]]
target_larm = [0.00, 0.25, 0.05, 0, 0, 0]
target_rarm = [0.00, - 0.25, 0.05, 0, 0, 0]
effectorList.append("LArm")
targetPos = almath.Position6D(target_larm)
pathList.append(list(targetPos.toVector()))
effectorList.append("RArm")
targetPos = almath.Position6D(target_rarm)
pathList.append(list(targetPos.toVector()))
motionProxy.positionInterpolations(effectorList, frame, pathList, axisMaskList, timeList)
time.sleep(1)
effectorList = []
pathList = []
timeLists = [3.0, 3.0]
target_larm = [ 0.00, 0.12, -0.10, 0, 0, 0]
target_rarm = [ 0.00, - 0.12, -0.10, 0, 0, 0]
effectorList.append("LArm")
targetPos = almath.Position6D(target_larm)
pathList.append(list(targetPos.toVector()))
effectorList.append("RArm")
targetPos = almath.Position6D(target_rarm)
pathList.append(list(targetPos.toVector()))
motionProxy.positionInterpolations(effectorList, frame, pathList, axisMaskList, timeList)
time.sleep(1)
'''
#names = ["LWristYaw", "RWristYaw", "LElbowYaw", "RElbowYaw"]
names = ["RElbowYaw", "RWristYaw"]
#angleLists = [left_ang_wrist_rad*almath.TO_RAD, right_ang_wrist_rad*almath.TO_RAD, left_ang_elbow_rad*almath.TO_RAD, right_ang_elbow_rad*almath.TO_RAD]
angleLists = [-45*almath.TO_RAD, -45*almath.TO_RAD]
#timeLists = [2.0, 2.0, 2.0, 2.0]
timeLists = [2.0, 2.0]
isAbsolute = True
motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute)
time.sleep(1)
'''
return GrabBallResponse(req.pos_ini)
def callback_point(self, point):
# Send robot to Pose Init
self.postureProxy.goToPosture("StandInit", 0.5)
frame = motion.FRAME_ROBOT # Establish the reference frame for the coordinates
useSensorValues = False
dx = 0.105 # translation axis X (meters)
dy = 0.05 # translation axis Y (meters)
if point.z > 0.2: # It limits what the arms raise upwards according to the height
dz = 0.10
else:
dz = 0.13 #(meters)
# Motion of Arms with block process
effectorList = []
pathList = []
axisMaskList = [motion.AXIS_MASK_VEL, motion.AXIS_MASK_VEL]
timeList = [[1.0], [1.0]] # seconds
## Move to the ball, but don't grab it yet
effectorList.append("LArm")
currentPos = self.motionProxy.getPosition("LArm", frame,
useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x += dx
targetPos.y -= (dy - 0.02)
targetPos.z += point.z
pathList.append(list(targetPos.toVector()))
effectorList.append("RArm")
currentPos = self.motionProxy.getPosition("RArm", frame,
useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x += dx
targetPos.y += (dy - 0.02)
targetPos.z += point.z
pathList.append(list(targetPos.toVector()))
self.motionProxy.positionInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
## Grab the ball
# Open Hands
self.motionProxy.openHand('LHand')
self.motionProxy.openHand('RHand')
pathList = []
currentPos = self.motionProxy.getPosition("LArm", frame,
useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.y -= (dy + 0.04)
pathList.append(list(targetPos.toVector()))
currentPos = self.motionProxy.getPosition("RArm", frame,
useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.y += (dy + 0.04)
pathList.append(list(targetPos.toVector()))
self.motionProxy.positionInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
## Lift arms a bit up and move to the robots chest to not hit the table and to be more stable while moving
pathList = []
currentPos = self.motionProxy.getPosition("LArm", frame,
useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x -= dx
targetPos.z += dz
pathList.append(list(targetPos.toVector()))
currentPos = self.motionProxy.getPosition("RArm", frame,
useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x -= dx
targetPos.z += dz
pathList.append(list(targetPos.toVector()))
self.motionProxy.positionInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
self.ttsProxy.say("¡Ya tengo la pelota!")
self.ttsProxy.say("Pero este color ya no me gusta")
## Throw the ball to the floor
pathList = []
currentPos = self.motionProxy.getPosition("LArm", frame,
useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x += dx
targetPos.z -= dz
pathList.append(list(targetPos.toVector()))
currentPos = self.motionProxy.getPosition("RArm", frame,
useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x += dx
targetPos.z -= dz
pathList.append(list(targetPos.toVector()))
self.motionProxy.positionInterpolations(effectorList, frame, pathList,
axisMaskList, timeList)
# End the game
self.end = True
self.Point_sub.unregister()
def callback(self,point):
# Send robot to Pose Init
self.postureProxy.goToPosture("StandInit", 0.5)
# Example showing how to use positionInterpolations
frame = motion.FRAME_ROBOT
useSensorValues = False
dx = 0.105 # translation axis X (meters)
dy = 0.05 # translation axis Y (meters)
if point.z > 0.2: # Limit arms movement according to the height
dz=0.10
else:
dz=0.13 #(meters)
# Motion of Arms with block process
effectorList = []
pathList = []
axisMaskList = [motion.AXIS_MASK_VEL, motion.AXIS_MASK_VEL]
timeList = [[1.0], [1.0]] # seconds
## Move to the ball, but don't grab it yet
effectorList.append("LArm")
currentPos = self.motionProxy.getPosition("LArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x += dx
targetPos.y -=(dy-0.02)
targetPos.z += point.z
pathList.append(list(targetPos.toVector()))
effectorList.append("RArm")
currentPos = self.motionProxy.getPosition("RArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x += dx
targetPos.y += (dy-0.02)
targetPos.z += point.z
pathList.append(list(targetPos.toVector()))
self.motionProxy.positionInterpolations(effectorList, frame, pathList, axisMaskList, timeList)
## Grab the ball
# OpenHands
self.motionProxy.openHand('LHand')
self.motionProxy.openHand('RHand')
pathList = []
#effectorList.append("LArm")
currentPos = self.motionProxy.getPosition("LArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
#targetPos.x += dx
targetPos.y -= (dy+0.02)
#targetPos.z += dz
pathList.append(list(targetPos.toVector()))
#effectorList.append("RArm")
currentPos = self.motionProxy.getPosition("RArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
#targetPos.x += dx
targetPos.y += (dy+0.02)
#targetPos.z += dz
pathList.append(list(targetPos.toVector()))
self.motionProxy.positionInterpolations(effectorList, frame, pathList, axisMaskList, timeList)
## Lift arms a bit up and move to the robots chest to not hit the table and to be more stable while moving
pathList = []
#effectorList.append("LArm")
currentPos = self.motionProxy.getPosition("LArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x -= dx
#targetPos.y -= dy
targetPos.z += dz
pathList.append(list(targetPos.toVector()))
#effectorList.append("RArm")
currentPos = self.motionProxy.getPosition("RArm", frame, useSensorValues)
targetPos = almath.Position6D(currentPos)
targetPos.x -= dx
#targetPos.y += dy
targetPos.z += dz
pathList.append(list(targetPos.toVector()))
self.motionProxy.positionInterpolations(effectorList, frame, pathList, axisMaskList, timeList)