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()
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()