def calc_jacob(angles):
    epsilon = 0.01
    pos, ori = k.left_arm_get_position(angles)
    for i, angle in enumerate(angles):
        samples = [a for a in angles]
        samples[i] = samples[i] + epsilon
        pos_, ori_ = k.left_arm_get_position(samples)
        error = pos_ - pos
        print error.transpose() / epsilon
def calc_jacob(angles):
    epsilon = 0.01
    pos, ori = k.left_arm_get_position(angles)
    for i, angle in enumerate(angles):
        samples = [a for a in angles]
        samples[i] = samples[i] + epsilon
        pos_, ori_ = k.left_arm_get_position(samples)
        error = pos_ - pos
        print error.transpose() / epsilon
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_fk_test(verbose=False):
    motion = naoqi.ALProxy("ALMotion", host, port)
    angle_set  = [ 
        [0] * 5,
        [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],
        [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.left_arm_tags, a, 1.0)
        time.sleep(2.0)
        pos0 = motion.getPosition("LArm", 0, False)
        angles = motion.getAngles(k.left_arm_tags, False)
        pos_, ori_ = k.left_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 left_arm_fk_test(verbose=False):
    motion = naoqi.ALProxy("ALMotion", host, port)
    angle_set = [
        [0] * 5,
        [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],
        [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.left_arm_tags, a, 1.0)
        time.sleep(2.0)
        pos0 = motion.getPosition("LArm", 0, False)
        angles = motion.getAngles(k.left_arm_tags, False)
        pos_, ori_ = k.left_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
示例#8
0
    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)
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)