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()
if mode == 'mayavi':
rospy.Subscriber('occupancy_grid', OccupancyGrid, mayavi_cb,
param_list)
while not rospy.is_shutdown():
if param_list[1] == True:
og3d = param_list[0]
print 'grid_shape:', og3d.grid.shape
pts = og3d.grid_to_points()
print pts.shape
# param_list[1] = False
break
rospy.sleep(0.1)
d3m.plot_points(pts)
d3m.show()
if mode == 'vis_occupancy':
rospy.Subscriber('occupancy_grid', OccupancyGrid, vis_occupancy_cb,
param_list)
import matplotlib_util.util as mpu
while not rospy.is_shutdown():
if param_list[1] == True:
og3d = param_list[0]
break
rospy.sleep(0.1)
occ_array = og3d.grid.flatten()
mpu.pl.hist(occ_array, 100)
# mpu.plot_yx(occ_array)
mpu.show()
elif mode == 'relay':
mode = 'vis_occupancy'
if mode == 'mayavi':
rospy.Subscriber('occupancy_grid', OccupancyGrid, mayavi_cb, param_list)
while not rospy.is_shutdown():
if param_list[1] == True:
og3d = param_list[0]
print 'grid_shape:', og3d.grid.shape
pts = og3d.grid_to_points()
print pts.shape
# param_list[1] = False
break
rospy.sleep(0.1)
d3m.plot_points(pts)
d3m.show()
if mode == 'vis_occupancy':
rospy.Subscriber('occupancy_grid', OccupancyGrid, vis_occupancy_cb, param_list)
import matplotlib_util.util as mpu
while not rospy.is_shutdown():
if param_list[1] == True:
og3d = param_list[0]
break
rospy.sleep(0.1)
occ_array = og3d.grid.flatten()
mpu.pl.hist(occ_array, 100)
# mpu.plot_yx(occ_array)
mpu.show()
elif mode == 'relay':
rospy.spin()