def plot_forces(combined_dict):
cd = combined_dict
hand_mat = np.column_stack(cd['hand_pos_list'])
hand_rot = cd['hand_rot_list']
mech_mat = np.column_stack(cd['mech_pos_list'])
mech_rot = cd['mech_rot_list']
directions_x = (np.row_stack(mech_rot)[:,0]).T.reshape(len(mech_rot), 3).T
force_cam, moment_cam, _ = fts_to_camera(combined_dict)
d3m.plot_points(hand_mat, color = (1.,0.,0.), mode='sphere')
d3m.plot_points(mech_mat, color = (0.,0.,1.), mode='sphere')
d3m.plot_normals(mech_mat, directions_x, color=(1.,0,0.))
# d3m.plot_normals(mech_mat, force_mat, color=(0.,1,0.))
d3m.plot_normals(mech_mat, force_cam, color=(0.,0,1.))
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()
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_trajectories(combined_dict):
cd = combined_dict
hand_mat = np.column_stack(cd['hand_pos_list'])
hand_rot = cd['hand_rot_list']
directions_x = (np.row_stack(hand_rot)[:,0]).T.reshape(len(hand_rot), 3).T
directions_y = (np.row_stack(hand_rot)[:,1]).T.reshape(len(hand_rot), 3).T
directions_z = (np.row_stack(hand_rot)[:,2]).T.reshape(len(hand_rot), 3).T
#d3m.white_bg()
d3m.plot_points(hand_mat, color = (1.,0.,0.), mode='sphere',
scale_factor = 0.005)
d3m.plot_normals(hand_mat, directions_x, color=(1.,0,0.),
scale_factor = 0.02)
d3m.plot_normals(hand_mat, directions_y, color=(0.,1,0.),
scale_factor = 0.02)
d3m.plot_normals(hand_mat, directions_z, color=(0.,0,1.),
scale_factor = 0.02)
mech_mat = np.column_stack(cd['mech_pos_list'])
mech_rot = cd['mech_rot_list']
directions_x = (np.row_stack(mech_rot)[:,0]).T.reshape(len(mech_rot), 3).T
directions_y = (np.row_stack(mech_rot)[:,1]).T.reshape(len(hand_rot), 3).T
directions_z = (np.row_stack(mech_rot)[:,2]).T.reshape(len(mech_rot), 3).T
d3m.plot_points(mech_mat[:,0:1], color = (0.,0.,0.), mode='sphere',
scale_factor = 0.01)
d3m.plot_points(mech_mat, color = (0.,0.,1.), mode='sphere',
scale_factor = 0.005)
d3m.plot_normals(mech_mat, directions_x, color=(1.,0,0.),
scale_factor = 0.02)
d3m.plot_normals(mech_mat, directions_y, color=(0.,1,0.),
scale_factor = 0.02)
d3m.plot_normals(mech_mat, directions_z, color=(0.,0,1.),
scale_factor = 0.02)
m = np.mean(mech_mat, 1)
d3m.mlab.view(azimuth=-120, elevation=60, distance=1.60,
focalpoint=(m[0,0], m[1,0], m[2,0]))
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()
#mode = 'mayavi'
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':
inv_mean_list.append(1. / m)
std_list.append(s)
x_l.append(s1)
y_l.append(s2)
z_l.append(s3)
ut.save_pickle(
{
'x_l': x_l,
'y_l': y_l,
'z_l': z_l,
'inv_mean_list': inv_mean_list,
'std_list': std_list
}, 'stiff_dict_' + ut.formatted_time() + '.pkl')
d3m.plot_points(np.matrix([x_l, y_l, z_l]),
scalar_list=inv_mean_list,
mode='sphere')
mlab.axes()
d3m.show()
sys.exit()
if fname == '':
print 'please specify a pkl file (-f option)'
print 'Exiting...'
sys.exit()
d = ut.load_pickle(fname)
actual_cartesian_tl = joint_to_cartesian(d['actual'], d['arm'])
actual_cartesian, _ = account_segway_motion(actual_cartesian_tl,
d['force'], d['segway'])
opt, args = p.parse_args()
if opt.dir != '':
poses_pkl = glob.glob(opt.dir + '/poses_dict*.pkl')[0]
d = ut.load_pickle(poses_pkl)
elif opt.fname != '':
d = ut.load_pickle(opt.fname)
else:
raise RuntimeError('need either -d or -f')
hand_list = d['hand']['pos_list']
hand_rot = d['hand']['rot_list']
if hand_list != []:
hand_mat = np.column_stack(hand_list)
print 'hand_mat.shape', hand_mat.shape
d3m.plot_points(hand_mat, color = (1.,0.,0.), mode='sphere')
directions_x = (np.row_stack(hand_rot)[:,0]).T.reshape(len(hand_rot), 3).T
directions_z = (np.row_stack(hand_rot)[:,2]).T.reshape(len(hand_rot), 3).T
#print 'directions.shape', directions_x.shape
#print 'hand_mat.shape', hand_mat.shape
#mpu.plot_yx(ut.norm(directions).A1, axis=None)
#mpu.show()
#mpu.plot_yx(ut.norm(hand_mat[:, 1:] - hand_mat[:, :-1]).A1, axis=None)
#mpu.show()
d3m.plot_normals(hand_mat, directions_x, color=(1.,0,0.))
d3m.plot_normals(hand_mat, directions_z, color=(0.,0,1.))
mechanism_list = d['mechanism']['pos_list']
for s2 in ratio_list2:
for s3 in ratio_list3:
i += 1
s_list = [s0,s1,s2,s3,0.8]
#s_list = [s1,s2,s3,s0,0.8]
print '################## s_list:', s_list
m,s = plot_stiff_ellipse_map(s_list,i)
inv_mean_list.append(1./m)
std_list.append(s)
x_l.append(s1)
y_l.append(s2)
z_l.append(s3)
ut.save_pickle({'x_l':x_l,'y_l':y_l,'z_l':z_l,'inv_mean_list':inv_mean_list,'std_list':std_list},
'stiff_dict_'+ut.formatted_time()+'.pkl')
d3m.plot_points(np.matrix([x_l,y_l,z_l]),scalar_list=inv_mean_list,mode='sphere')
mlab.axes()
d3m.show()
sys.exit()
if fname=='':
print 'please specify a pkl file (-f option)'
print 'Exiting...'
sys.exit()
d = ut.load_pickle(fname)
actual_cartesian = joint_to_cartesian(d['actual'])
eq_cartesian = joint_to_cartesian(d['eq_pt'])
for p in actual_cartesian.p_list:
