示例#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()
示例#2
0
def calc_motion_all():
    for i in range(len(ee_tl.p_list)):
        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)
        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]
        vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
                            start_pos_ts,eq_pt_tl,bndry)
        print 'angle:',math.degrees(a)
示例#3
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()
示例#4
0
    def segway_motion_local(self, curr_pos_tl, sa):
        k = self.wkd['pts'].keys()
        z = min(self.eq_pt_cartesian[2,0], curr_pos_tl[2,0])
        k_idx = np.argmin(np.abs(np.array(k)-z))
        wrkspc_pts = self.wkd['pts'][k[k_idx]]
        bndry = self.wkd['bndry'][k[k_idx]]

        self.eq_pt_close_to_bndry = False
        if z < -0.4:
            # don't open the mechanism
            self.eq_motion_vec = np.matrix([0.,0.,0.]).T
            self.eq_pt_close_to_bndry = True

        if self.zenither_client.height() > 0.6:
            if z < -0.35 and self.zenither_moving == False:
                #self.z.nadir(1)
                self.z.nadir(0)
                self.zenither_moving = True
            if z > -0.25 and self.zenither_moving == True:
                self.z.estop()
                self.zenither_moving = False
        else:
            if self.zenither_moving:
                self.z.estop()
                self.zenither_moving = False

        ht = smc.segway_motion_repulse(curr_pos_tl, self.eq_pt_cartesian, bndry,
                                       wrkspc_pts)

        self.vec_bndry = smc.vec_from_boundary(self.eq_pt_cartesian, bndry)
        self.dist_boundary = smc.dist_from_boundary(self.eq_pt_cartesian, bndry,
                                                    wrkspc_pts)
        dist_bndry_ee = smc.dist_from_boundary(curr_pos_tl,bndry,wrkspc_pts)
        self.vec_bndry = self.vec_bndry/self.dist_boundary
        vec_bndry = self.vec_bndry
        dist_boundary = self.dist_boundary

        avel = -sa * 0.05 # maintaining the orientation of the segway.

        eq_mec = mcf.mecanumTglobal(mcf.globalTtorso(self.eq_pt_cartesian))
        svel_rot_y = -avel*eq_mec[0,0] # segway vel in y direction due to rotation
        svel_rot_x = avel*eq_mec[1,0] # segway vel in x direction due to rotation

        if self.zenither_moving:
            sp = 0.075
        else:
            sp = 0.075
        st = ht*sp + np.matrix([svel_rot_x,svel_rot_y]).T
        st[0,0] = min(st[0,0],0.) # disallowing +ve translation.

        self.move_segway_lock.acquire()
        self.segway_motion_tup = (st[0,0],st[1,0],avel)
        self.new_segway_command = True
        self.move_segway_lock.release()

        proj = (self.eq_motion_vec[0:2,0].T*vec_bndry)[0,0]
        proj_threshold = math.cos(math.radians(100))
        if (dist_boundary<= 0.04 and proj<proj_threshold) or \
            dist_boundary<0.03:
            #dist_boundary<0.01 or dist_bndry_ee < 0.04:
            self.eq_motion_vec = np.matrix([0.,0.,0.]).T
            self.eq_pt_close_to_bndry = True

        if self.eq_pt_close_to_bndry:
            if not self.zenither_moving:
                self.eq_pt_not_moving_counter += 1
            else:
                self.eq_pt_not_moving_counter = 0
        else:
            self.eq_pt_not_moving_counter = 0


        print '-------------------------------------'
        print 'segway_translation:',st[0,0],st[1,0]
        print 'avel:',math.degrees(avel)
        print 'dist_boundary:', dist_boundary
        print 'proj,proj_threshold:',proj,proj_threshold
        print 'self.eq_pt_cartesian:',self.eq_pt_cartesian.A1.tolist()
        print 'self.eq_pt_close_to_bndry?',self.eq_pt_close_to_bndry
        print 'dist_bndry_ee:',dist_bndry_ee