コード例 #1
0
def plot_err_histogram(rel_list, abs_list, title):
    # plot relative error histogram.
    max_err = 1.0
    bin_width = 0.05  # relative err.
    bins = np.arange(0. - bin_width / 2. - max_err, max_err + 2 * bin_width,
                     bin_width)
    hist, bin_edges = np.histogram(np.array(rel_list), bins)
    mpu.figure()
    mpu.plot_histogram(bin_edges[:-1] + bin_width / 2.,
                       hist,
                       width=bin_width * 0.8,
                       xlabel='Relative Error',
                       plot_title=title)
    # plot relative error histogram.
    max_err = 20
    bin_width = 2  # relative err.
    bins = np.arange(0. - bin_width / 2. - max_err, max_err + 2 * bin_width,
                     bin_width)
    hist, bin_edges = np.histogram(np.array(abs_list), bins)
    mpu.figure()
    mpu.plot_histogram(bin_edges[:-1] + bin_width / 2.,
                       hist,
                       width=bin_width * 0.8,
                       xlabel='Absolute Error',
                       plot_title=title)
コード例 #2
0
def plot_workspace(pts, ha, z):
    if pts.shape[1] == 0:
        return
    mpu.figure()
    good_location = pts.mean(1)
    mpu.plot_yx(pts[1, :].A1, pts[0, :].A1, label="ha:%.1f" % (math.degrees(ha)), axis="equal", linewidth=0)
    mpu.plot_yx(good_location[1, :].A1, good_location[0, :].A1, axis="equal", linewidth=0, scatter_size=90, color="k")
    mpu.savefig("z%.2f_ha%.1f.png" % (z, math.degrees(ha)))
コード例 #3
0
ファイル: workspace_selector.py プロジェクト: wklharry/hrl
def plot_workspace(pts, ha, z):
    if pts.shape[1] == 0:
        return
    mpu.figure()
    good_location = pts.mean(1)
    mpu.plot_yx(pts[1, :].A1,
                pts[0, :].A1,
                label='ha:%.1f' % (math.degrees(ha)),
                axis='equal',
                linewidth=0)
    mpu.plot_yx(good_location[1, :].A1,
                good_location[0, :].A1,
                axis='equal',
                linewidth=0,
                scatter_size=90,
                color='k')
    mpu.savefig('z%.2f_ha%.1f.png' % (z, math.degrees(ha)))
コード例 #4
0
def plot_err_histogram(rel_list, abs_list, title):
    # plot relative error histogram.
    max_err = 1.0
    bin_width = 0.05 # relative err.
    bins = np.arange(0.-bin_width/2.-max_err, max_err+2*bin_width, bin_width)
    hist, bin_edges = np.histogram(np.array(rel_list), bins)
    mpu.figure()
    mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist,
                       width=bin_width*0.8, xlabel='Relative Error',
                       plot_title=title)
    # plot relative error histogram.
    max_err = 20
    bin_width = 2 # relative err.
    bins = np.arange(0.-bin_width/2.-max_err, max_err+2*bin_width, bin_width)
    hist, bin_edges = np.histogram(np.array(abs_list), bins)
    mpu.figure()
    mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist,
                       width=bin_width*0.8, xlabel='Absolute Error',
                       plot_title=title)
コード例 #5
0
def plot_radial_tangential(mech_dict, savefig, fig_name=''):
    radial_mech = mech_dict['force_rad_list']
    tangential_mech = mech_dict['force_tan_list']
    typ = mech_dict['mech_type']
    mech_x = mech_dict['mechanism_x']
    if typ == 'rotary':
        mech_x_degrees = np.degrees(mech_x)
        xlabel = 'angle (degrees)'
    else:
        mech_x_degrees = mech_x
        xlabel = 'distance (meters)'
    mpu.pl.clf()
    mpu.plot_yx(radial_mech, mech_x_degrees, axis=None, label='radial force',
                xlabel=xlabel, ylabel='N', color='r')
    mpu.plot_yx(tangential_mech, mech_x_degrees, axis=None, label='tangential force',
                xlabel=xlabel, ylabel='N', color='g')
    mpu.legend()
    
    if typ == 'rotary':
        mpu.figure()
        rad = mech_dict['radius']
        torques_1 = rad * np.array(tangential_mech)
        torques_3 = np.array(mech_dict['moment_tip_list']) + torques_1
        mpu.plot_yx(torques_1, mech_x_degrees, axis=None,
                    label='torque from tangential',
                    xlabel=xlabel, ylabel='moment', color='r')
        mpu.plot_yx(torques_3, mech_x_degrees, axis=None,
                    label='total torque',
                    xlabel=xlabel, ylabel='moment', color='y')
        mpu.legend()


    if savefig:
        mpu.savefig(opt.dir+'/%s_force_components.png'%fig_name)
    else:
        mpu.show()
コード例 #6
0
ファイル: arm_trajectories.py プロジェクト: wklharry/hrl
        print 'Estimated rad, cx, cy:', rad, cx, cy

        start_angle = tr.angle_within_mod180(
            math.atan2(pts_2d[1, 0] - cy, pts_2d[0, 0] - cx) - math.pi / 2)
        end_angle = tr.angle_within_mod180(
            math.atan2(pts_2d[1, -1] - cy, pts_2d[0, -1] - cx) - math.pi / 2)

        subsample_ratio = 1
        pts_2d_s = pts_2d[:, ::subsample_ratio]

        cep_force_clean, force_new = filter_trajectory_force(
            eq_cartesian, d['force'])
        cep_2d = np.matrix(cep_force_clean.p_list).T[0:2, reject_idx:]

        # first draw the entire CEP and end effector trajectories
        mpu.figure()
        mpu.plot_yx(pts_2d_s[1, :].A1,
                    pts_2d_s[0, :].A1,
                    color='b',
                    label='\huge{End Effector Trajectory}',
                    axis='equal',
                    alpha=1.0,
                    scatter_size=7,
                    linewidth=0,
                    marker='x',
                    marker_edge_width=1.5)

        cep_2d_s = cep_2d[:, ::subsample_ratio]
        mpu.plot_yx(cep_2d_s[1, :].A1,
                    cep_2d_s[0, :].A1,
                    color='g',
コード例 #7
0
ファイル: segway_motion_calc.py プロジェクト: gt-ros-pkg/hrl
    z = -0.23
    k = d.keys()
    k_idx = np.argmin(np.abs(np.array(k)-z))
    pts = d[k[k_idx]]
#    visualize_boundary()

    for kk in k:
        pts = d[kk]
        bpts = compute_boundary(pts)
        cx,cy = 0.7,-0.6
        rad = 0.4
        
    #    pts_in = point_contained(cx,cy,0.,rad,pts,
    #                             start_angle=math.radians(140),
    #                             end_angle=math.radians(190))
        mpu.figure()
        mpu.plot_yx(pts[1,:].A1,pts[0,:].A1,linewidth=0)
        mpu.plot_yx(bpts[1,:].A1,bpts[0,:].A1,linewidth=0,color='y')
#    mpu.plot_yx(pts_in[1,:].A1,pts_in[0,:].A1,linewidth=0,color='g')
#    mpu.plot_yx([cy],[cx],linewidth=0,color='r')
    mpu.show()




### apologies for the poor name. computes the translation and rotation
## of the torso frame that move the eq pt away from closest boundary
## and rotate such that local x axis is perp to mechanism
## returns 2x1 np matrix, angle
#def segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,start_pos_ts,
#                          eq_pt_tl,bndry):
コード例 #8
0
ファイル: plot_csv.py プロジェクト: gt-ros-pkg/hrl
def plot_radii(csv_data, color='#3366FF'):
    keys = ['radius', 'type', 'name', 'repetition']
    llists = expand(extract_keys(csv_data, keys), keys)
    rad_list = np.array([float(r) for r in llists[0]]) / 100.0
    types = llists[1]
    names = np.array(llists[2])
    all_types = set(types)
    print 'Radii types', all_types

    types_arr = np.array(types)
    # np.where(types_arr == 'C')
    cabinet_rad_list = rad_list[np.where(types_arr == 'C')[0]]
    others_rad_list = rad_list[np.where(types_arr != 'C')[0]]
    other_names = names[np.where(types_arr != 'C')[0]]
    print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
    print 'radii other names'
    for i, n in enumerate(other_names):
        print n, others_rad_list[i]
    print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'

    rad_lists = [rad_list, cabinet_rad_list, others_rad_list]
    titles = ['Radii of Rotary Mechanisms', 'Radii of Cabinets', 'Radii of Other Mechanisms']
    bin_width = 0.05
    max_radius = np.max(rad_list)
    print 'MIN RADIUS', np.min(rad_list)
    print 'MAX RADIUS', max_radius

    mpu.set_figure_size(5.,5.)
    for idx, radii in enumerate(rad_lists):
        f = pb.figure()
        f.set_facecolor('w')
        bins = np.arange(0.-bin_width/2., max_radius+2*bin_width, bin_width)
        hist, bin_edges = np.histogram(radii, bins)
        h = mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist,
                           width=0.8*bin_width, xlabel='Radius (meters)',
                           ylabel='\# of mechanisms',
                           plot_title=titles[idx],
                           color=color, label='All')
        pb.xlim(.1, 1.)
        pb.ylim(0, 55)
        mpu.legend(display_mode = 'less_space', handlelength=1.)

    #-- different classes in different colors in the same histogram.
    f = pb.figure()
    f.set_facecolor('w')
    bins = np.arange(0.-bin_width/2., max_radius+2*bin_width, bin_width)
    hist, bin_edges = np.histogram(rad_lists[0], bins)
    h = mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist,
                       width=0.8*bin_width, xlabel='Radius (meters)',
                       ylabel='\# of mechanisms',
                       plot_title=titles[1],
                       color='g', label='Cabinets')
    hist, bin_edges = np.histogram(rad_lists[2], bins)
    h = mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist,
                       width=0.8*bin_width, xlabel='Radius (meters)',
                       ylabel='\# of mechanisms',
                       plot_title='Cabinets and Other Mechanisms',
                       color='y', label='Other')
    pb.xlim(.1, 1.)
    pb.ylim(0, 55)
    mpu.legend(display_mode = 'less_space', handlelength=1.)

    color_list = ['g', 'b', 'r']
    marker_list = ['s', '^', 'v']
    label_list = ['All', 'Cabinets', 'Other']
    scatter_size_list = [8, 5, 5]
    mpu.set_figure_size(5.,5.)
    mpu.figure()
    for idx, radii in enumerate(rad_lists):
        bins = np.arange(0.-bin_width/2., max_radius+2*bin_width, bin_width)
        hist, bin_edges = np.histogram(radii, bins)
        bin_midpoints = np.arange(0., max_radius+bin_width, bin_width)
        mpu.plot_yx(hist, bin_midpoints, color = color_list[idx],
                    alpha = 0.6, marker = marker_list[idx],
                    scatter_size = scatter_size_list[idx], xlabel='Radius (meters)',
                    ylabel='\# of mechanisms', label = label_list[idx])
    mpu.legend(display_mode = 'less_space')
コード例 #9
0
def plot_radii(csv_data, color='#3366FF'):
    keys = ['radius', 'type', 'name', 'repetition']
    llists = expand(extract_keys(csv_data, keys), keys)
    rad_list = np.array([float(r) for r in llists[0]]) / 100.0
    types = llists[1]
    names = np.array(llists[2])
    all_types = set(types)
    print 'Radii types', all_types

    types_arr = np.array(types)
    # np.where(types_arr == 'C')
    cabinet_rad_list = rad_list[np.where(types_arr == 'C')[0]]
    others_rad_list = rad_list[np.where(types_arr != 'C')[0]]
    other_names = names[np.where(types_arr != 'C')[0]]
    print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
    print 'radii other names'
    for i, n in enumerate(other_names):
        print n, others_rad_list[i]
    print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'

    rad_lists = [rad_list, cabinet_rad_list, others_rad_list]
    titles = [
        'Radii of Rotary Mechanisms', 'Radii of Cabinets',
        'Radii of Other Mechanisms'
    ]
    bin_width = 0.05
    max_radius = np.max(rad_list)
    print 'MIN RADIUS', np.min(rad_list)
    print 'MAX RADIUS', max_radius

    mpu.set_figure_size(5., 5.)
    for idx, radii in enumerate(rad_lists):
        f = pb.figure()
        f.set_facecolor('w')
        bins = np.arange(0. - bin_width / 2., max_radius + 2 * bin_width,
                         bin_width)
        hist, bin_edges = np.histogram(radii, bins)
        h = mpu.plot_histogram(bin_edges[:-1] + bin_width / 2.,
                               hist,
                               width=0.8 * bin_width,
                               xlabel='Radius (meters)',
                               ylabel='\# of mechanisms',
                               plot_title=titles[idx],
                               color=color,
                               label='All')
        pb.xlim(.1, 1.)
        pb.ylim(0, 55)
        mpu.legend(display_mode='less_space', handlelength=1.)

    #-- different classes in different colors in the same histogram.
    f = pb.figure()
    f.set_facecolor('w')
    bins = np.arange(0. - bin_width / 2., max_radius + 2 * bin_width,
                     bin_width)
    hist, bin_edges = np.histogram(rad_lists[0], bins)
    h = mpu.plot_histogram(bin_edges[:-1] + bin_width / 2.,
                           hist,
                           width=0.8 * bin_width,
                           xlabel='Radius (meters)',
                           ylabel='\# of mechanisms',
                           plot_title=titles[1],
                           color='g',
                           label='Cabinets')
    hist, bin_edges = np.histogram(rad_lists[2], bins)
    h = mpu.plot_histogram(bin_edges[:-1] + bin_width / 2.,
                           hist,
                           width=0.8 * bin_width,
                           xlabel='Radius (meters)',
                           ylabel='\# of mechanisms',
                           plot_title='Cabinets and Other Mechanisms',
                           color='y',
                           label='Other')
    pb.xlim(.1, 1.)
    pb.ylim(0, 55)
    mpu.legend(display_mode='less_space', handlelength=1.)

    color_list = ['g', 'b', 'r']
    marker_list = ['s', '^', 'v']
    label_list = ['All', 'Cabinets', 'Other']
    scatter_size_list = [8, 5, 5]
    mpu.set_figure_size(5., 5.)
    mpu.figure()
    for idx, radii in enumerate(rad_lists):
        bins = np.arange(0. - bin_width / 2., max_radius + 2 * bin_width,
                         bin_width)
        hist, bin_edges = np.histogram(radii, bins)
        bin_midpoints = np.arange(0., max_radius + bin_width, bin_width)
        mpu.plot_yx(hist,
                    bin_midpoints,
                    color=color_list[idx],
                    alpha=0.6,
                    marker=marker_list[idx],
                    scatter_size=scatter_size_list[idx],
                    xlabel='Radius (meters)',
                    ylabel='\# of mechanisms',
                    label=label_list[idx])
    mpu.legend(display_mode='less_space')
コード例 #10
0
ファイル: workspace_selector.py プロジェクト: wklharry/hrl
    mpu.plot_yx(good_location[1, :].A1,
                good_location[0, :].A1,
                axis='equal',
                linewidth=0,
                scatter_size=90,
                color='k')
    mpu.savefig('z%.2f_ha%.1f.png' % (z, math.degrees(ha)))


argv = sys.argv
fname = sys.argv[1]
dd = ut.load_pickle(fname)

color_list = ['b', 'y', 'g']
i = 0
mpu.figure(dpi=100)
for ha in dd.keys():
    d = dd[ha]
    l = []
    key_list = d['pts'].keys()
    for k in key_list:
        pts = d['pts'][k]
        l.append(pts.shape[1])
        #plot_workspace(pts,ha,k)

    ll = zip(key_list, l)
    ll.sort()

    key_list, l = zip(*ll)
    if ha == 0:
        label = 'Hook Left'
コード例 #11
0
def split_forces_hooktip_test(hand_mat):
    kin_dict = ke.fit(hand_mat, tup)
    center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
                             kin_dict['cz'])).T

    print 'kin_dict:', kin_dict
    # radial vectors.
    radial_mat = hand_mat - center_hand
    radial_mat = radial_mat / ut.norm(radial_mat)

    # cannot use hook tip to compute mechanism angle because I
    # don't have a way of knowing when the hook starts opening the
    # mechanism. (Think hook makes contact with door, moves in
    # freespace and then makes contact with the handle.)
    #start_rad = radial_mat[:,0]
    #ct = (start_rad.T * radial_mat).A1
    #st = ut.norm(np.matrix(np.cross(start_rad.A1, radial_mat.T.A)).T).A1
    #mech_angle_l = np.arctan2(st, ct).tolist()

    _, nrm_hand = project_points_plane(hand_mat)
    print 'nrm_hand:', nrm_hand.A1

    f_cam_l = []
    m_cam_l = []
    m_base_l = []
    frad_l = []
    ftan_l = []
    hook_num = cd['hook_checker_number']
    print 'hook_num:', hook_num
    for i, f in enumerate(force_mat.T):
        f = f.T
        m = moment_mat[:,i]
        f_cam, m_cam, m_base = ft_to_camera_3(f, m, hook_rot_l[i], hook_num,
                                              return_moment_cam = True)
        f_cam_l.append(f_cam)
        m_cam_l.append(abs((m_cam.T*nrm_hand)[0,0]))
        m_base_l.append(abs((m_base.T*nrm_hand)[0,0]))
        #m_base_l.append(np.linalg.norm(f))

        tangential_vec = np.matrix(np.cross(radial_mat[:,i].A1, nrm_hand.A1)).T
        ftan = (f_cam.T * tangential_vec)[0,0]
        ftan_l.append(ftan)

        #frad = np.linalg.norm(f_cam - tangential_vec * ftan)
        frad = (f_cam.T*radial_mat[:,i])[0,0]
        frad_l.append(abs(frad))


    fig1 = mpu.figure()
    mech_ang_deg = np.degrees(mech_angle_l)
    mpu.plot_yx(ftan_l, mech_ang_deg, label='Tangential Force (hook tip)', color='b')
    mpu.plot_yx(frad_l, mech_ang_deg, label='Radial Force (hook tip)', color='y')

    mech_pkl_name = glob.glob(opt.dir + '/open_mechanism_trajectories_*.pkl')[0]
    md = ut.load_pickle(mech_pkl_name)
    radial_mech = md['force_rad_list']
    tangential_mech = md['force_tan_list']
    mech_x = np.degrees(md['mechanism_x'])
    mpu.plot_yx(tangential_mech, mech_x, label='Tangential Force (mechanism checker)', color='g')
    mpu.plot_yx(radial_mech, mech_x, label='Radial Force (mechanism checker)', color='r')


    mpu.legend()

    fig2 = mpu.figure()
    mpu.plot_yx(m_cam_l, mech_ang_deg, label='\huge{$m_{axis}$}')
    mpu.plot_yx(m_base_l, mech_ang_deg, label='\huge{$m^s$}',
                color = 'r')

    mpu.legend()
    mpu.show()
コード例 #12
0
        cd = ut.load_pickle(glob.glob(opt.dir + '/combined_log*.pkl')[0])

        tup = ke.init_ros_node()

        ma2 = compute_mech_angle_2(cd, tup, project_plane=False)
        ma1 = compute_mech_angle_1(cd)
        ma3 = compute_mech_angle_2(cd, tup, project_plane=True)
#        ma4 = compute_mech_angle_1(cd, min_evec)


        lab1 = 'orientation only'
        lab2 = 'checker origin position + circle fit'
        lab3 = 'checker origin position + PCA projection + circle fit'
#        lab4 = 'PCA projection + orientation'

        mpu.figure()
        mpu.plot_yx(np.degrees(ma3), color='r', label=lab3,
                    linewidth = 1, scatter_size = 5)
        mpu.plot_yx(np.degrees(ma2), color='b', label=lab2,
                    linewidth = 1, scatter_size = 5)
        mpu.plot_yx(np.degrees(ma1), color='y', label=lab1,
                    linewidth = 1, scatter_size = 5)
        mpu.legend()

        vel3 = ma.compute_velocity(ma3, cd['time_list'], 1)
        vel2 = ma.compute_velocity(ma2, cd['time_list'], 1)
        vel1 = ma.compute_velocity(ma1, cd['time_list'], 1)
        mpu.figure()
        mpu.plot_yx(np.degrees(vel3), np.degrees(ma3), color='r',
                    label=lab3, linewidth = 1, scatter_size = 5)
        mpu.plot_yx(np.degrees(vel2), np.degrees(ma2), color='b',
コード例 #13
0
    if pts.shape[1] == 0:
        return
    mpu.figure()
    good_location = pts.mean(1)
    mpu.plot_yx(pts[1, :].A1, pts[0, :].A1, label="ha:%.1f" % (math.degrees(ha)), axis="equal", linewidth=0)
    mpu.plot_yx(good_location[1, :].A1, good_location[0, :].A1, axis="equal", linewidth=0, scatter_size=90, color="k")
    mpu.savefig("z%.2f_ha%.1f.png" % (z, math.degrees(ha)))


argv = sys.argv
fname = sys.argv[1]
dd = ut.load_pickle(fname)

color_list = ["b", "y", "g"]
i = 0
mpu.figure(dpi=100)
for ha in dd.keys():
    d = dd[ha]
    l = []
    key_list = d["pts"].keys()
    for k in key_list:
        pts = d["pts"][k]
        l.append(pts.shape[1])
        # plot_workspace(pts,ha,k)

    ll = zip(key_list, l)
    ll.sort()

    key_list, l = zip(*ll)
    if ha == 0:
        label = "Hook Left"