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.))
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']
mechanism_rot = d['mechanism']['rot_list']
#mechanism_list = []
if mechanism_list != []:
mechanism_mat = np.column_stack(mechanism_list)
print 'mechanism_mat.shape', mechanism_mat.shape
d3m.plot_points(mechanism_mat, color = (0.,0.,1.), mode='sphere')
#d3m.plot_points(mechanism_mat, color = (0.,0.,1.), mode='sphere')
c = np.row_stack(mechanism_rot)[:,2]
directions = c.T.reshape(len(mechanism_rot), 3).T
print 'directions.shape', directions.shape
print 'mechanism_mat.shape', mechanism_mat.shape
d3m.plot_normals(mechanism_mat, directions, color=(0,0,1.))
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]))