def IsInCollision(x, obc, obc_width=6.): STATE_THETA_1, STATE_THETA_2, STATE_V_1, STATE_V_2 = 0, 1, 2, 3 MIN_V_1, MAX_V_1 = -6., 6. MIN_V_2, MAX_V_2 = -6., 6. MIN_TORQUE, MAX_TORQUE = -4., 4. MIN_ANGLE, MAX_ANGLE = -np.pi, np.pi LENGTH = 20. m = 1.0 lc = 0.5 lc2 = 0.25 l2 = 1. I1 = 0.2 I2 = 1.0 l = 1.0 g = 9.81 pole_x0 = 0. pole_y0 = 0. pole_x1 = LENGTH * np.cos(x[STATE_THETA_1] - np.pi / 2) pole_y1 = LENGTH * np.sin(x[STATE_THETA_1] - np.pi / 2) pole_x2 = pole_x1 + LENGTH * np.cos(x[STATE_THETA_1] + x[STATE_THETA_2] - np.pi / 2) pole_y2 = pole_y1 + LENGTH * np.sin(x[STATE_THETA_1] + x[STATE_THETA_2] - np.pi / 2) for i in range(len(obc)): for j in range(0, 8, 2): x1 = obc[i][j] y1 = obc[i][j+1] x2 = obc[i][(j+2) % 8] y2 = obc[i][(j+3) % 8] if line_line_cc(pole_x0, pole_y0, pole_x1, pole_y1, x1, y1, x2, y2): return True if line_line_cc(pole_x1, pole_y1, pole_x2, pole_y2, x1, y1, x2, y2): return True return False
def start_goal_gen(low, high, width, obs, obs_recs): # using obs information and bound, to generate good start and goal width = 4. H = 0.5 L = 2.5 while True: start = np.random.uniform(low=low, high=high) # set the position to be within smaller bound ratio = .8 start[0] = start[0] * ratio start_x0 = start[0] start_y0 = H start_x1 = start[0] + L * np.sin(start[2]) start_y1 = H + L * np.cos(start[2]) cf = True for i in range(len(obs_recs)): for j in range(4): if line_line_cc(start_x0, start_y0, start_x1, start_y1, obs_recs[i][j][0], obs_recs[i][j][1], obs_recs[i][(j + 1) % 4][0], obs_recs[i][(j + 1) % 4][1]): cf = False break while True: end = np.random.uniform(low=low, high=high) end[0] = end[0] * ratio end_x0 = end[0] end_y0 = H end_x1 = end[0] + L * np.sin(end[2]) end_y1 = H + L * np.cos(end[2]) cf = True for i in range(len(obs_recs)): for j in range(4): if line_line_cc(end_x0, end_y0, end_x1, end_y1, obs_recs[i][j][0], obs_recs[i][j][1], obs_recs[i][(j + 1) % 4][0], obs_recs[i][(j + 1) % 4][1]): cf = False break if cf: break if np.abs(start[0] - end[0]) >= width * 6: break start[1] = 0. start[3] = 0. return start, end
def IsInCollision(x, obc, obc_width=4.): I = 10 L = 2.5 M = 10 m = 5 g = 9.8 H = 0.5 STATE_X = 0 STATE_V = 1 STATE_THETA = 2 STATE_W = 3 CONTROL_A = 0 MIN_X = -30 MAX_X = 30 MIN_V = -40 MAX_V = 40 MIN_W = -2 MAX_W = 2 if x[0] < MIN_X or x[0] > MAX_X: return True H = 0.5 pole_x1 = x[0] pole_y1 = H pole_x2 = x[0] + L * np.sin(x[2]) pole_y2 = H + L * np.cos(x[2]) for i in range(len(obc)): for j in range(0, 8, 2): x1 = obc[i][j] y1 = obc[i][j+1] x2 = obc[i][(j+2) % 8] y2 = obc[i][(j+3) % 8] if line_line_cc(pole_x1, pole_y1, pole_x2, pole_y2, x1, y1, x2, y2): return True return False
def start_goal_gen(low, high, width, obs, obs_recs): # using obs information and bound, to generate good start and goal LENGTH = 20. near = width * 1.2 s_g_dis_threshold = LENGTH * 1.6 start = np.zeros(4) # here we fix the start end = np.zeros(4) """ while True: start[0] = np.random.uniform(low=low[0], high=high[0]) # make sure start midpoint x0_start = 0. y0_start = 0. x1_start = LENGTH * np.cos(start[0] - np.pi/2) y1_start = LENGTH * np.sin(start[0] - np.pi/2) #print('x1 = %f, y1 = %f' % (x1_start, y1_start)) # make sure (x0, y0) to (x1, y1) is collision free cf = True for i in range(len(obs_recs)): for j in range(4): if line_line_cc(x0_start, y0_start, x1_start, y1_start, obs_recs[i][j][0], obs_recs[i][j][1], obs_recs[i][(j+1)%4][0], obs_recs[i][(j+1)%4][1]): cf = False break if cf: break #print('generated start point') while True: start[1] = np.random.uniform(low=low[1], high=high[1]) # make sure start and end not in collision x2_start = LENGTH * np.cos(start[0] + start[1] - np.pi/2) + x1_start y2_start = LENGTH * np.sin(start[0] + start[1] - np.pi/2) + y1_start # make sure (x0, y0) to (x1, y1) is collision free cf = True for i in range(len(obs_recs)): for j in range(4): if line_line_cc(x1_start, y1_start, x2_start, y2_start, obs_recs[i][j][0], obs_recs[i][j][1], obs_recs[i][(j+1)%4][0], obs_recs[i][(j+1)%4][1]): cf = False break if cf: break """ x0_start = 0. y0_start = 0. x1_start = LENGTH * np.cos(start[0] - np.pi / 2) y1_start = LENGTH * np.sin(start[0] - np.pi / 2) x2_start = LENGTH * np.cos(start[0] + start[1] - np.pi / 2) + x1_start y2_start = LENGTH * np.sin(start[0] + start[1] - np.pi / 2) + y1_start # start endpoint while True: #print('generated endpoint') end[0] = np.random.uniform(low=low[0], high=high[0]) # make sure start midpoint x0_goal = 0. y0_goal = 0. x1_goal = LENGTH * np.cos(end[0] - np.pi / 2) y1_goal = LENGTH * np.sin(end[0] - np.pi / 2) # start endpoint end[1] = np.random.uniform(low=low[1], high=high[1]) # make sure start and end not in collision x2_goal = LENGTH * np.cos(end[0] + end[1] - np.pi / 2) + x1_goal y2_goal = LENGTH * np.sin(end[0] + end[1] - np.pi / 2) + y1_goal # make sure y2 is always positive, if not, then the mirrow image will be positive if y2_goal < 0.: end[0] = np.pi - end[0] end[1] = -end[1] x1_goal = LENGTH * np.cos(end[0] - np.pi / 2) y1_goal = LENGTH * np.sin(end[0] - np.pi / 2) x2_goal = LENGTH * np.cos(end[0] + end[1] - np.pi / 2) + x1_goal y2_goal = LENGTH * np.sin(end[0] + end[1] - np.pi / 2) + y1_goal # check if y2_goal is high enough if y2_goal <= LENGTH: continue # make sure the acrobot is collision free cf = True for i in range(len(obs_recs)): for j in range(4): if line_line_cc(x0_goal, y0_goal, x1_goal, y1_goal, obs_recs[i][j][0], obs_recs[i][j][1], obs_recs[i][(j + 1) % 4][0], obs_recs[i][(j + 1) % 4][1]): cf = False break if line_line_cc(x1_goal, y1_goal, x2_goal, y2_goal, obs_recs[i][j][0], obs_recs[i][j][1], obs_recs[i][(j + 1) % 4][0], obs_recs[i][(j + 1) % 4][1]): cf = False break if not cf: break # need to be in different phase, i.e. at least one sign need to be different if np.linalg.norm( np.array([x2_goal, y2_goal]) - np.array([x2_start, y2_start])) < s_g_dis_threshold: continue # add endpoint length constraint if cf: break #start_sign = np.array([x2_start, y2_start]) >= 0 #end_sign = np.array([x2_goal, y2_goal]) >= 0 #if start_sign[0] != end_sign[0] or start_sign[1] != end_sign[1]: # #print('sg generated.') # break print('feasible sg generated.') start[2] = 0. start[3] = 0. end[2] = 0. end[3] = 0. return start, end