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
