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()
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