Example #1
0
def plot_hook_translation(curr_pos_tl,cx_tl,cy_tl,cy_ts,
                          start_pos_ts,eq_pt_tl,bndry,wrkspc_pts):
    vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
                        start_pos_ts,eq_pt_tl,bndry,wrkspc_pts)


    mpu.plot_yx(eq_pt_tl[1,:].A1, eq_pt_tl[0,:].A1, linewidth=2,
                color='g', scatter_size=15, label='Eq Pt')
    mpu.plot_yx(curr_pos_tl[1,:].A1, curr_pos_tl[0,:].A1, linewidth=0,
                color='b', scatter_size = 15, label = 'FK')
    mpu.plot_yx(bndry[1,:].A1, bndry[0,:].A1, linewidth=0, color='y',
                scatter_size=8)
    mpu.plot_yx([-0.2], [0.], linewidth=0, color='b', scatter_size=2)

    bndry_dist_eq = smc.dist_from_boundary(eq_pt_tl, bndry, wrkspc_pts) # signed
    bndry_dist_ee = smc.dist_from_boundary(curr_pos_tl, bndry, wrkspc_pts) # signed
    if bndry_dist_ee < bndry_dist_eq:
        p = curr_pos_tl
    else:
        p = eq_pt_tl
    pts_close = smc.pts_within_dist(p[0:2,:],bndry,0.01,0.1)
    mpu.plot_yx(pts_close[1,:].A1, pts_close[0,:].A1, linewidth=0,
                color='r', scatter_size = 8)

    nrm = np.linalg.norm(vt)
    vt = vt/nrm
    mpu.plot_quiver_yxv(p[1,:].A1, p[0,:].A1, vt, scale=12)

    mpu.show()
Example #2
0
def plot_cartesian(traj, xaxis=None, yaxis=None, zaxis=None, color='b',label='_nolegend_',
                   linewidth=2, scatter_size=10, plot_velocity=False):

    import arm_trajectories as at
    #if traj.__class__ == at.JointTrajectory:
    if isinstance(traj,at.JointTrajectory):
        traj = joint_to_cartesian(traj)

    pts = np.matrix(traj.p_list).T
    label_list = ['X coord (m)', 'Y coord (m)', 'Z coord (m)']
    x = pts[xaxis,:].A1.tolist()
    y = pts[yaxis,:].A1.tolist()

    if plot_velocity:
        vels = np.matrix(traj.v_list).T
        xvel = vels[xaxis,:].A1.tolist()
        yvel = vels[yaxis,:].A1.tolist()

    if zaxis == None:
        mpu.plot_yx(y, x, color, linewidth, '-', scatter_size, label,
                    axis = 'equal', xlabel = label_list[xaxis],
                    ylabel = label_list[yaxis],)
        if plot_velocity:
            mpu.plot_quiver_yxv(y, x, np.matrix([xvel,yvel]),
                                width = 0.001, scale = 1.)
        mpu.legend()
    else:
        from numpy import array
        from enthought.mayavi.api import Engine
        engine = Engine()
        engine.start()
        if len(engine.scenes) == 0:
            engine.new_scene()

        z = pts[zaxis,:].A1.tolist()
        time_list = [t-traj.time_list[0] for t in traj.time_list]
        mlab.plot3d(x,y,z,time_list,tube_radius=None,line_width=4)
        mlab.axes()
        mlab.xlabel(label_list[xaxis])
        mlab.ylabel(label_list[yaxis])
        mlab.zlabel(label_list[zaxis])
        mlab.colorbar(title='Time')

        # ------------------------------------------- 
        axes = engine.scenes[0].children[0].children[0].children[1]
        axes.axes.position = array([ 0.,  0.])
        axes.axes.label_format = '%-#6.2g'
        axes.title_text_property.font_size=4
Example #3
0
def plot_cartesian(traj, xaxis=None, yaxis=None, zaxis=None, color='b',label='_nolegend_',
                   linewidth=2, scatter_size=10, plot_velocity=False):
    import matplotlib_util.util as mpu
    import arm_trajectories as at
    #if traj.__class__ == at.JointTrajectory:
    if isinstance(traj,at.JointTrajectory):
        traj = joint_to_cartesian(traj)

    pts = np.matrix(traj.p_list).T
    label_list = ['X coord (m)', 'Y coord (m)', 'Z coord (m)']
    x = pts[xaxis,:].A1.tolist()
    y = pts[yaxis,:].A1.tolist()

    if plot_velocity:
        vels = np.matrix(traj.v_list).T
        xvel = vels[xaxis,:].A1.tolist()
        yvel = vels[yaxis,:].A1.tolist()

    if zaxis == None:
        mpu.plot_yx(y, x, color, linewidth, '-', scatter_size, label,
                    axis = 'equal', xlabel = label_list[xaxis],
                    ylabel = label_list[yaxis],)
        if plot_velocity:
            mpu.plot_quiver_yxv(y, x, np.matrix([xvel,yvel]),
                                width = 0.001, scale = 1.)
        mpu.legend()
    else:
        from numpy import array
        from enthought.mayavi.api import Engine
        engine = Engine()
        engine.start()
        if len(engine.scenes) == 0:
            engine.new_scene()

        z = pts[zaxis,:].A1.tolist()
        time_list = [t-traj.time_list[0] for t in traj.time_list]
        mlab.plot3d(x,y,z,time_list,tube_radius=None,line_width=4)
        mlab.axes()
        mlab.xlabel(label_list[xaxis])
        mlab.ylabel(label_list[yaxis])
        mlab.zlabel(label_list[zaxis])
        mlab.colorbar(title='Time')

        # ------------------------------------------- 
        axes = engine.scenes[0].children[0].children[0].children[1]
        axes.axes.position = array([ 0.,  0.])
        axes.axes.label_format = '%-#6.2g'
        axes.title_text_property.font_size=4
Example #4
0
def plot_eq_pt_motion_tl():
    vec_list = []
    for i in range(len(ee_tl.p_list)):
#    for i in range(5):
        curr_pos_tl = np.matrix(ee_tl.p_list[i]).T
        eq_pt_tl = np.matrix(eq_tl.p_list[i]).T

        pts_ts = np.matrix(ee_ts.p_list[0:i+1]).T
        pts_2d_ts = pts_ts[0:2,:]
#        rad_opt,cx_ts,cy_ts = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d_ts,
#                                         method='fmin_bfgs',verbose=False)
        rad_opt = 1.0
        cx_ts,cy_ts = 0.5,-1.3
        c_ts = np.matrix([cx_ts,cy_ts,0.]).T
        x,y,a = st.x_list[i],st.y_list[i],st.a_list[i]
        c_tl = smc.tlTts(c_ts,x,y,a)
        cx_tl,cy_tl = c_tl[0,0],c_tl[1,0]
        t0 = time.time()
        vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
                            start_pos_ts,eq_pt_tl,bndry,wrkspc_pts)
        t1 = time.time()
#        print 'time to segway_motion_repulse:',t1-t0
        nrm = np.linalg.norm(vt)
#        if nrm > 0.005:
        vt = vt/nrm
        vec_list.append(vt.A1.tolist())

    v = np.matrix(vec_list).T

    eq_pts = np.matrix(eq_tl.p_list).T
    ee_pts = np.matrix(ee_tl.p_list).T
    mpu.plot_yx(eq_pts[1,:].A1,eq_pts[0,:].A1,linewidth=1,color='g',label='eq')
    mpu.plot_yx(ee_pts[1,:].A1,ee_pts[0,:].A1,linewidth=1,color='b',label='FK')
    mpu.plot_yx(bndry[1,:].A1,bndry[0,:].A1,linewidth=0,color='y')
    mpu.plot_quiver_yxv(eq_pts[1,:].A1,eq_pts[0,:].A1,v,scale=30)
    mpu.legend()
    mpu.show()
Example #5
0
                        end_angle,
                        label='\huge{Estimated Kinematics}',
                        color='r',
                        alpha=0.7)
        mpu.pl.xlim(0.28, 0.47)
        mpu.legend()
        #mpu.savefig('three.png')
        mpu.savefig(fig_name + '%d' % fig_number)
        fig_number += 1

        current_pos = pts_2d_zoom[:, -1]
        radial_vec = current_pos - np.matrix([cx, cy]).T
        radial_vec = radial_vec / np.linalg.norm(radial_vec)
        tangential_vec = np.matrix([[0, -1], [1, 0]]) * radial_vec
        mpu.plot_quiver_yxv([pts_2d_zoom[1, -1]], [pts_2d_zoom[0, -1]],
                            radial_vec,
                            scale=10.,
                            width=0.002)
        rad_text_loc = pts_2d_zoom[:, -1] + np.matrix([0.001, 0.01]).T
        mpu.pl.text(rad_text_loc[0, 0],
                    rad_text_loc[1, 0],
                    '$\hat v_{rad}$',
                    fontsize=30)
        mpu.plot_quiver_yxv([pts_2d_zoom[1, -1]], [pts_2d_zoom[0, -1]],
                            tangential_vec,
                            scale=10.,
                            width=0.002)

        tan_text_loc = pts_2d_zoom[:, -1] + np.matrix([-0.012, -0.011]).T
        mpu.pl.text(tan_text_loc[0, 0],
                    tan_text_loc[1, 0],
                    s='$\hat v_{tan}$',
Example #6
0
                               pts_2d_zoom[0,-1]-cx) - math.pi/2)
        mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
                        label='\huge{Estimated Kinematics}', color='r',
                        alpha=0.7)
        mpu.pl.xlim(0.28, 0.47)
        mpu.legend()
        #mpu.savefig('three.png')
        mpu.savefig(fig_name+'%d'%fig_number)
        fig_number += 1

        current_pos = pts_2d_zoom[:,-1]
        radial_vec = current_pos - np.matrix([cx,cy]).T
        radial_vec = radial_vec / np.linalg.norm(radial_vec)
        tangential_vec = np.matrix([[0,-1],[1,0]]) * radial_vec
        mpu.plot_quiver_yxv([pts_2d_zoom[1,-1]],
                            [pts_2d_zoom[0,-1]],
                            radial_vec, scale=10., width = 0.002)
        rad_text_loc = pts_2d_zoom[:,-1] + np.matrix([0.001,0.01]).T
        mpu.pl.text(rad_text_loc[0,0], rad_text_loc[1,0], '$\hat v_{rad}$', fontsize = 30)
        mpu.plot_quiver_yxv([pts_2d_zoom[1,-1]],
                            [pts_2d_zoom[0,-1]],
                            tangential_vec, scale=10., width = 0.002)

        tan_text_loc = pts_2d_zoom[:,-1] + np.matrix([-0.012, -0.011]).T
        mpu.pl.text(tan_text_loc[0,0], tan_text_loc[1,0], s = '$\hat v_{tan}$', fontsize = 30)
        mpu.pl.xlim(0.28, 0.47)
        mpu.legend()
        #mpu.savefig('four.png')
        mpu.savefig(fig_name+'%d'%fig_number)
        fig_number += 1