Пример #1
0
def compare_tip_mechanism_trajectories(mech_mat, hand_mat):
#    hand_proj, nrm_hand = project_points_plane(hand_mat)
#    mech_proj, nrm_mech = project_points_plane(mech_mat)
    hand_proj = hand_mat
    mech_proj = mech_mat

    kin_dict = ke.fit(hand_proj, tup)
    print 'kin_dict from hook tip:', kin_dict
    print 'measured radius:', cd['radius']
    center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
                             kin_dict['cz'])).T

    kin_dict = ke.fit(mech_proj, tup)
    print 'kin_dict from mechanism:', kin_dict
    center_mech = np.matrix((kin_dict['cx'], kin_dict['cy'],
                             kin_dict['cz'])).T

    # working with the projected coordinates.
    directions_hand = hand_proj - center_hand
    directions_hand = directions_hand / ut.norm(directions_hand)
    directions_mech = mech_proj - center_mech
    directions_mech = directions_mech / ut.norm(directions_mech)

    start_normal = directions_mech[:,0]
    print 'directions_mech[:,0]', directions_mech[:,0].A1
    print 'directions_hand[:,0]', directions_hand[:,0].A1
    ct = (start_normal.T * directions_hand).A1
    st = ut.norm(np.matrix(np.cross(start_normal.A1, directions_hand.T.A)).T).A1
    
    mech_angle = np.arctan2(st, ct).tolist()
    #mech_angle = np.arccos(start_normal.T * directions_hand).A1.tolist()

    mpu.plot_yx(np.degrees(mech_angle))
    mpu.show()

    # plot trajectory in 3D.
    d3m.white_bg()
    d3m.plot_points(hand_proj, color = (1.,0.,0.), mode='sphere',
                    scale_factor = 0.005)
    d3m.plot_points(mech_proj[:,0:1], color = (0.,0.,0.), mode='sphere',
                    scale_factor = 0.01)
    d3m.plot_points(mech_proj, color = (0.,0.,1.), mode='sphere',
                    scale_factor = 0.005)
    d3m.plot(np.column_stack((mech_proj[:,-1],center_mech, mech_proj[:,0])),
             color = (0.,0.,1.))
    d3m.plot(np.column_stack((hand_proj[:,-1],center_hand, hand_proj[:,0])),
             color = (1.,0.,0.))
    d3m.show()
Пример #2
0
def angle_between_hooktip_mechanism_radial_vectors(mech_mat, hand_mat):
    kin_dict = ke.fit(hand_mat, tup)
    print 'kin_dict from hook tip:', kin_dict
    print 'measured radius:', cd['radius']
    center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
                             kin_dict['cz'])).T

    kin_dict = ke.fit(mech_mat, tup)
    print 'kin_dict from mechanism:', kin_dict
    center_mech = np.matrix((kin_dict['cx'], kin_dict['cy'],
                             kin_dict['cz'])).T

    # working with the projected coordinates.
    directions_hand = hand_mat - center_hand
    directions_hand = directions_hand / ut.norm(directions_hand)
    directions_mech = mech_mat - center_mech
    directions_mech = directions_mech / ut.norm(directions_mech)

    #import pdb; pdb.set_trace()
    ang = np.degrees(np.arccos(np.sum(np.multiply(directions_mech, directions_hand), 0))).A1
    mpu.plot_yx(ang, label = 'angle between hooktip-radial and mechanism radial')
    mpu.legend()
    mpu.show()
Пример #3
0
def compute_mech_angle_2(cd, tup, project_plane=False):
    pos_mat = np.column_stack(cd['mech_pos_list'])
    if project_plane:
        pts_proj, min_evec = project_points_plane(pos_arr)
        pos_arr = pts_proj

    kin_dict = ke.fit(pos_mat, tup)
    center = np.matrix((kin_dict['cx'], kin_dict['cy'],
                       kin_dict['cz'])).T
    directions_x = pos_mat - center
    directions_x = directions_x / ut.norm(directions_x)
    start_normal = directions_x[:,0]
    #mech_angle = np.arccos(start_normal.T * directions_x).A1.tolist()

    ct = (start_normal.T * directions_x).A1
    st = ut.norm(np.matrix(np.cross(start_normal.A1, directions_x.T.A)).T).A1
    mech_angle = np.arctan2(st, ct).tolist()
    return mech_angle
Пример #4
0
def compute_mech_rot_list(cd, tup, project_plane=False):
    pos_arr = np.column_stack(cd['mech_pos_list'])
    rot_list = cd['mech_rot_list']
    directions_y = (np.row_stack(rot_list)[:,1]).T.reshape(len(rot_list), 3).T
    start_y = directions_y[:,0]

    pts_proj, y_mech = project_points_plane(pos_arr)
    if np.dot(y_mech.T, start_y.A1) < 0:
        print 'Negative hai boss'
        y_mech = -1 * y_mech

    if project_plane:
        pos_arr = pts_proj

    kin_dict = ke.fit(np.matrix(pos_arr), tup)
    center = np.array((kin_dict['cx'], kin_dict['cy'],
                       kin_dict['cz'])).T

    print 'pos_arr[:,0]', pos_arr[:,0]
    print 'center:', center

    directions_x = (np.row_stack(rot_list)[:,0]).T.reshape(len(rot_list), 3).T
    start_x = directions_x[:,0]
    directions_x = np.matrix(pos_arr) - np.matrix(center).T.A
    directions_x = directions_x / ut.norm(directions_x)
    if np.dot(directions_x[:,0].A1, start_x.A1) < 0:
        print 'Negative hai boss'
        directions_x = -1 * directions_x

    mech_rot_list = []
    for i in range(len(rot_list)):
        x = -directions_x[:, i]
        y = np.matrix(y_mech)
        z = np.matrix(np.cross(x.A1, y.A1)).T
        rot_mat = np.column_stack((x, y, z))
        mech_rot_list.append(rot_mat)

#    return mech_rot_list
    return rot_list
Пример #5
0
def split_forces_hooktip_test(hand_mat):
    kin_dict = ke.fit(hand_mat, tup)
    center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
                             kin_dict['cz'])).T

    print 'kin_dict:', kin_dict
    # radial vectors.
    radial_mat = hand_mat - center_hand
    radial_mat = radial_mat / ut.norm(radial_mat)

    # cannot use hook tip to compute mechanism angle because I
    # don't have a way of knowing when the hook starts opening the
    # mechanism. (Think hook makes contact with door, moves in
    # freespace and then makes contact with the handle.)
    #start_rad = radial_mat[:,0]
    #ct = (start_rad.T * radial_mat).A1
    #st = ut.norm(np.matrix(np.cross(start_rad.A1, radial_mat.T.A)).T).A1
    #mech_angle_l = np.arctan2(st, ct).tolist()

    _, nrm_hand = project_points_plane(hand_mat)
    print 'nrm_hand:', nrm_hand.A1

    f_cam_l = []
    m_cam_l = []
    m_base_l = []
    frad_l = []
    ftan_l = []
    hook_num = cd['hook_checker_number']
    print 'hook_num:', hook_num
    for i, f in enumerate(force_mat.T):
        f = f.T
        m = moment_mat[:,i]
        f_cam, m_cam, m_base = ft_to_camera_3(f, m, hook_rot_l[i], hook_num,
                                              return_moment_cam = True)
        f_cam_l.append(f_cam)
        m_cam_l.append(abs((m_cam.T*nrm_hand)[0,0]))
        m_base_l.append(abs((m_base.T*nrm_hand)[0,0]))
        #m_base_l.append(np.linalg.norm(f))

        tangential_vec = np.matrix(np.cross(radial_mat[:,i].A1, nrm_hand.A1)).T
        ftan = (f_cam.T * tangential_vec)[0,0]
        ftan_l.append(ftan)

        #frad = np.linalg.norm(f_cam - tangential_vec * ftan)
        frad = (f_cam.T*radial_mat[:,i])[0,0]
        frad_l.append(abs(frad))


    fig1 = mpu.figure()
    mech_ang_deg = np.degrees(mech_angle_l)
    mpu.plot_yx(ftan_l, mech_ang_deg, label='Tangential Force (hook tip)', color='b')
    mpu.plot_yx(frad_l, mech_ang_deg, label='Radial Force (hook tip)', color='y')

    mech_pkl_name = glob.glob(opt.dir + '/open_mechanism_trajectories_*.pkl')[0]
    md = ut.load_pickle(mech_pkl_name)
    radial_mech = md['force_rad_list']
    tangential_mech = md['force_tan_list']
    mech_x = np.degrees(md['mechanism_x'])
    mpu.plot_yx(tangential_mech, mech_x, label='Tangential Force (mechanism checker)', color='g')
    mpu.plot_yx(radial_mech, mech_x, label='Radial Force (mechanism checker)', color='r')


    mpu.legend()

    fig2 = mpu.figure()
    mpu.plot_yx(m_cam_l, mech_ang_deg, label='\huge{$m_{axis}$}')
    mpu.plot_yx(m_base_l, mech_ang_deg, label='\huge{$m^s$}',
                color = 'r')

    mpu.legend()
    mpu.show()
Пример #6
0
def compute_mechanism_properties(combined_dict, bias_ft = False,
                                 tup = None, cd_pkl_name = None):
    cd = combined_dict
    force_cam, moment_cam, _ = fts_to_camera(combined_dict)
    moment_contact_l = None
    if bias_ft:
        print 'Biasing magnitude:', np.linalg.norm(force_cam[:,0])
        force_cam = force_cam - force_cam[:,0]
        moment_cam = moment_cam - moment_cam[:,0]

    if cd['radius'] != -1:
        if tup == None:
            mech_angle = compute_mech_angle_1(cd)
        else:
            mech_angle = compute_mech_angle_2(cd, tup)
            # compute new mech_rot_list. used for factoring the forces.
            mech_rot_list = compute_mech_rot_list(combined_dict, tup)
            combined_dict['mech_rot_list'] = mech_rot_list

            hook_tip_l = compute_hook_tip_trajectory(cd)
            hand_mat = np.column_stack(hook_tip_l)
            for i,f in enumerate(force_cam.T):
                fmag = np.linalg.norm(f)
                if fmag > 1.0:
                    break
            end_idx = np.argmax(mech_angle)
            hand_mat_short = hand_mat[:,i:end_idx]

            kin_dict = ke.fit(hand_mat_short, tup)
            center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
                                     kin_dict['cz'])).T
            radius_hand = kin_dict['r']
            
            center_mech_coord = mech_rot_list[0].T * center_hand
            start_mech_coord = mech_rot_list[0].T * hand_mat_short[:,0]
            opens_right = False
            if start_mech_coord[0,0] < center_mech_coord[0,0]:
                print 'Opens Right'
                opens_right = True

            # radial vectors.
            radial_mat = hand_mat - center_hand
            radial_mat = radial_mat / ut.norm(radial_mat)
            _, nrm_hand = project_points_plane(hand_mat_short)
            if np.dot(nrm_hand.A1, mech_rot_list[0][:,1].A1) < 0:
                nrm_hand = -1 * nrm_hand
            if opens_right == False:
                nrm_hand = -1 * nrm_hand

            frad_l = []
            ftan_l = []
            moment_contact_l = []
            for i, f in enumerate(force_cam.T):
                f = f.T
                m = (moment_cam[:,i].T * nrm_hand)[0,0]
                moment_contact_l.append(m)
                tvec = np.matrix(np.cross(nrm_hand.A1, radial_mat[:,i].A1)).T
                ftan = (f.T * tvec)[0,0]
                ftan_l.append(ftan)

                frad = np.linalg.norm(f - tvec * ftan)
                #frad = (f_cam.T*radial_mat[:,i])[0,0]
                frad_l.append(abs(frad))

        typ = 'rotary'

    else:
        pos_mat = np.column_stack(cd['mech_pos_list'])
        mech_angle = ut.norm(pos_mat-pos_mat[:,0]).A1.tolist()
        #print 'mech_angle:', mech_angle
        typ = 'prismatic'
        moment_axis_list = None

        frad_l = []
        ftan_l = []
        moment_contact_l = []
        rot_list = cd['mech_rot_list']
        directions_z = (np.row_stack(rot_list)[:,2]).T.reshape(len(rot_list), 3).T
        for i, f in enumerate(force_cam.T):
            f = f.T
            tvec = np.matrix(directions_z[:,i])
            ftan = (f.T * tvec)[0,0]
            ftan_l.append(ftan)

            frad = np.linalg.norm(f - tvec * ftan)
            #frad = (f_cam.T*radial_mat[:,i])[0,0]
            frad_l.append(abs(frad))
            radius_hand = 10.

    ut.save_pickle(combined_dict, cd_pkl_name)

    return np.array(frad_l), np.array(ftan_l), mech_angle, typ, \
           np.array(ftan_l)*radius_hand, np.array(moment_contact_l)