def left_arm_ik_continuous(verbose=False):
work_pose = k.left_arm_work_pose
pos, ori = k.left_arm_get_position(work_pose)
poss = []
d = 32
r = 0.05
time.sleep(5)
poss = []
pos[1] = pos[1] + r
poss.append(np.copy(pos))
p = np.copy(pos)
p[1] = p[1] + r
poss.append(p)
poss.append(np.copy(pos))
p = np.copy(pos)
p[1] = p[1] - r
poss.append(p)
poss.append(np.copy(pos))
p = np.copy(pos)
p[2] = p[2] - r
poss.append(p)
poss.append(np.copy(pos))
p = np.copy(pos)
p[2] = p[2] + r
poss.append(p)
poss.append(np.copy(pos))
motion = naoqi.ALProxy("ALMotion", host, port)
print k.left_arm_work_pose
motion.setAngles(k.left_arm_tags, work_pose, 1.0)
time.sleep(1.0)
for i in range(2):
for p in poss:
angles = motion.getAngles(k.left_arm_tags, True)
target_angles = k.left_arm_set_position(angles, p, ori)
a = [a for a in target_angles]
motion.setAngles(k.left_arm_tags, a, 1.0)
time.sleep(0.5)
poss = []
for i in range(d):
p = np.copy(pos)
th = math.pi * 2 / d * i
p[1] = p[1] + r * math.cos(th)
p[2] = p[2] + r * math.sin(th)
poss.append(p)
motion = naoqi.ALProxy("ALMotion", host, port)
print k.left_arm_work_pose
motion.setAngles(k.left_arm_tags, work_pose, 1.0)
time.sleep(1.0)
for i in range(5):
for p in poss:
angles = motion.getAngles(k.left_arm_tags, True)
target_angles = k.left_arm_set_position(angles, p, ori)
a = [a for a in target_angles]
motion.setAngles(k.left_arm_tags, a, 1.0)
time.sleep(0.1)
def left_arm_ik_continuous(verbose=False):
work_pose = k.left_arm_work_pose
pos, ori = k.left_arm_get_position(work_pose)
poss = []
d = 32
r = 0.05
time.sleep(5)
poss = []
pos[1] = pos[1] + r
poss.append(np.copy(pos))
p = np.copy(pos)
p[1] = p[1] + r
poss.append(p)
poss.append(np.copy(pos))
p = np.copy(pos)
p[1] = p[1] - r
poss.append(p)
poss.append(np.copy(pos))
p = np.copy(pos)
p[2] = p[2] - r
poss.append(p)
poss.append(np.copy(pos))
p = np.copy(pos)
p[2] = p[2] + r
poss.append(p)
poss.append(np.copy(pos))
motion = naoqi.ALProxy("ALMotion", host, port)
print k.left_arm_work_pose
motion.setAngles(k.left_arm_tags, work_pose, 1.0)
time.sleep(1.0)
for i in range(2):
for p in poss:
angles = motion.getAngles(k.left_arm_tags, True)
target_angles = k.left_arm_set_position(angles, p, ori)
a = [a for a in target_angles]
motion.setAngles(k.left_arm_tags, a, 1.0)
time.sleep(0.5)
poss = []
for i in range(d):
p = np.copy(pos)
th = math.pi * 2 / d * i
p[1] = p[1] + r * math.cos(th)
p[2] = p[2] + r * math.sin(th)
poss.append(p)
motion = naoqi.ALProxy("ALMotion", host, port)
print k.left_arm_work_pose
motion.setAngles(k.left_arm_tags, work_pose, 1.0)
time.sleep(1.0)
for i in range(5):
for p in poss:
angles = motion.getAngles(k.left_arm_tags, True)
target_angles = k.left_arm_set_position(angles, p, ori)
a = [a for a in target_angles]
motion.setAngles(k.left_arm_tags, a, 1.0)
time.sleep(0.1)
def left_arm_ik_test(verbose=False):
angle_set = [
[0] * 5,
k.left_arm_initial_pose,
k.left_arm_work_pose]
pos, ori = k.left_arm_get_position(angle_set[2])
a = angle_set[1]
delta = angle_set[2] - k.left_arm_set_position(a, pos,ori)
print np.sum(delta*delta)
def left_arm_ik_test(verbose=False):
angle_set = [[0] * 5, k.left_arm_initial_pose, k.left_arm_work_pose]
pos, ori = k.left_arm_get_position(angle_set[2])
a = angle_set[1]
delta = angle_set[2] - k.left_arm_set_position(a, pos, ori)
print np.sum(delta * delta)
