Ejemplo n.º 1
0
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.))
Ejemplo n.º 2
0
    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.))
Ejemplo n.º 3
0
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]))