示例#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 plot_hooktip_trajectory_and_force(cd):
    hook_tip_l = compute_hook_tip_trajectory(cd)

    # plot trajectory in 3D.
    d3m.white_bg()
    d3m.plot_points(np.column_stack(hook_tip_l), 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()