def right_arm_ik_test(verbose=False): angle_set = [[0] * 5, k.right_arm_initial_pose, k.right_arm_work_pose] pos, ori = k.right_arm_get_position(angle_set[2]) a = angle_set[1] delta = angle_set[2] - k.right_arm_set_position(a, pos, ori) print np.sum(delta * delta)
def right_arm_ik_test(verbose=False): angle_set = [ [0] * 5, k.right_arm_initial_pose, k.right_arm_work_pose] pos, ori = k.right_arm_get_position(angle_set[2]) a = angle_set[1] delta = angle_set[2] - k.right_arm_set_position(a, pos,ori) print np.sum(delta*delta)
def right_arm_fk_test(): motion = naoqi.ALProxy("ALMotion", host, port) angle_set = [ [0] * 5, [1.57, 0, 0, 0, 0], [0, 0, 1.57, 0, 0], [1.57, 0, 0, 0, 0], [0, 0, 0, 0, 1.57], [0, 0, 0, 1.57, 0], [0, 0, 1.57, 1.57, 0], [1.57, -1.57, 0, 0, 0], [1.57, -1.57, 0, 0, -1.57], [1.57, -1.57, 0, 0, 1.57], [1.57, 0, 0, 0, 0], ] for a in angle_set: motion.setAngles(k.right_arm_tags, a, 1.0) time.sleep(2.0) pos0 = motion.getPosition("RArm", 0, True) angles = motion.getAngles(k.right_arm_tags, True) pos_, ori_ = k.right_arm_get_position(angles) pos = pos0[0:3] pos[0] = pos[0] + 0.057 pos[2] = pos[2] - 0.08682 pos_error = [p - p_ for p, p_ in zip(pos, pos_)] sum_pos_error = 0 for p in pos_error: sum_pos_error = sum_pos_error + math.fabs(p) ori = pos0[3:] Tx = k.transX(ori[0], 0, 0, 0) Ty = k.transY(-ori[1], 0, 0, 0) Tz = k.transZ(ori[2], 0, 0, 0) T = Tz.dot(Ty.dot(Tx))[0:3, 0:3] T_ = ori_ ori_error = T - T_ sum_ori_error = 0 for o in ori_error: for v in o: sum_ori_error = sum_ori_error + math.fabs(v) pass print 'Error(a, pos ori), ', a, ', ', sum_pos_error, ', ', sum_ori_error
def right_arm_fk_test(): motion = naoqi.ALProxy("ALMotion", host, port) angle_set = [ [0] * 5, [1.57, 0, 0, 0, 0], [0, 0, 1.57, 0, 0], [1.57, 0, 0, 0, 0], [0, 0, 0, 0, 1.57], [0, 0, 0, 1.57, 0], [0, 0, 1.57, 1.57, 0], [1.57, -1.57, 0, 0, 0], [1.57, -1.57, 0, 0, -1.57], [1.57, -1.57, 0, 0, 1.57], [1.57, 0, 0, 0, 0], ] for a in angle_set: motion.setAngles(k.right_arm_tags, a, 1.0) time.sleep(2.0) pos0 = motion.getPosition("RArm", 0, True) angles = motion.getAngles(k.right_arm_tags, True) pos_, ori_ = k.right_arm_get_position(angles) pos = pos0[0:3] pos[0] = pos[0] + 0.057 pos[2] = pos[2] - 0.08682 pos_error = [p - p_ for p, p_ in zip(pos, pos_)] sum_pos_error = 0 for p in pos_error: sum_pos_error = sum_pos_error + math.fabs(p) ori = pos0[3:] Tx = k.transX(ori[0],0,0,0) Ty = k.transY(-ori[1],0,0,0) Tz = k.transZ(ori[2],0,0,0) T = Tz.dot(Ty.dot(Tx))[0:3,0:3] T_ = ori_ ori_error = T - T_ sum_ori_error = 0 for o in ori_error: for v in o: sum_ori_error = sum_ori_error + math.fabs(v) pass print 'Error(a, pos ori), ', a, ', ', sum_pos_error, ', ', sum_ori_error
def get_actual_position(self, shoulder_pitch, shoulder_roll, elbow_yaw, elbow_roll, right_arm=False): """ get actual position of Pepper's hands returns a list [x, y, z] """ if right_arm: actual_wrist, _ = pk.right_arm_get_position( [shoulder_pitch, shoulder_roll, elbow_yaw, elbow_roll, 0]) else: actual_wrist, _ = pk.left_arm_get_position( [shoulder_pitch, shoulder_roll, elbow_yaw, elbow_roll, 0]) actual_wrist[ 2] += self.pepper_head_to_base_link_z # pk outputs position with respect to head frame, not base_link frame actual_wrist[ 0] += self.pepper_head_to_base_link_x # pk outputs position with respect to head frame, not base_link frame actual_wrist = actual_wrist[: -1] # pepper_kinematics has an extra 1 at the end that's not needed return list(actual_wrist)