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)