def Gdubins3D(start_x, start_y, start_z, goal_x, goal_y, goal_z, rad, mat2Dconvert3D,mat3Dconvert2D):
p2_x, p2_y, p2_z = pnt3DConvert2Pnt2D(goal_x - start_x, goal_y - start_y, goal_z - start_z, mat3Dconvert2D)
print(p2_x,p2_y,p2_z)
# raise Exception('x')
px, py, pyaw, mode, clen = db.dubins_path_planning(0,0,0,p2_x, p2_y,rad, 1000)
pnt3D_xLst = []
pnt3D_yLst = []
pnt3D_zLst = []
for i in range(len(px)):
p3_x,p3_y,p3_z = pnt2DConvert2Pnt3D(px[i],py[i],mat2Dconvert3D)
pnt3D_xLst.append(p3_x)
pnt3D_yLst.append(p3_y)
pnt3D_zLst.append(p3_z)
pnt3D_xLst = [x + start_x for x in pnt3D_xLst]
pnt3D_yLst = [y + start_y for y in pnt3D_yLst]
pnt3D_zLst = [z + start_z for z in pnt3D_zLst]
print(p2_x)
print(p2_y)
print(pyaw[-1])
p2_x_step = p2_x[0] + 1000*cos(pyaw[-1])
p2_y_step = p2_y[0] + 1000*sin(pyaw[-1])
print(p2_x_step,p2_y_step)
# exit()
print('mat2Dconvert3D', mat2Dconvert3D)
p3_x, p3_y, p3_z = pnt2DConvert2Pnt3D(p2_x_step, p2_y_step, mat2Dconvert3D)
print('mat2Dconvert3D', mat2Dconvert3D)
p3_x = p3_x + start_x
p3_y = p3_y + start_y
p3_z = p3_z + start_z
bias_x = p3_x - goal_x
bias_y = p3_y - goal_y
bias_z = p3_z - goal_z
a_gama = math.atan(bias_z/2000)
if bias_y >0:
a_psi = math.atan(bias_y/bias_x)
else:
a_psi = math.atan(bias_y/bias_x) + math.pi
# py = [y + start_y for y in py]
return pnt3D_xLst,pnt3D_yLst,pnt3D_zLst,psi,gama, p3_x, p3_y, p3_z
def Gdubins3D(start_x, start_y, start_z, goal_x, goal_y, goal_z, rad,
mat2Dconvert3D, mat3Dconvert2D):
p2_x, p2_y, p2_z = pnt3DConvert2Pnt2D(goal_x - start_x, goal_y - start_y,
goal_z - start_z, mat3Dconvert2D)
print(p2_x, p2_y, p2_z)
# raise Exception('x')
px, py, pyaw, mode, dis_d = db.dubins_path_planning(
0, 0, 0, p2_x, p2_y, rad, 200)
print(mode)
print(dis_d)
pnt3D_xLst = []
pnt3D_yLst = []
pnt3D_zLst = []
for i in range(len(px)):
p3_x, p3_y, p3_z = pnt2DConvert2Pnt3D(px[i], py[i], mat2Dconvert3D)
pnt3D_xLst.append(p3_x)
pnt3D_yLst.append(p3_y)
pnt3D_zLst.append(p3_z)
pnt3D_xLst = [x + start_x for x in pnt3D_xLst]
pnt3D_yLst = [y + start_y for y in pnt3D_yLst]
pnt3D_zLst = [z + start_z for z in pnt3D_zLst]
p2_x_step = p2_x + cos(pyaw)
p2_y_step = p2_y + sin(pyaw)
p3_x, p3_y, p3_z = pnt2DConvert2Pnt3D(p2_x_step, p2_x_step, mat2Dconvert3D)
goal_dir_x = p3_x - goal_x
goal_dir_y = p3_y - goal_y
goal_dir_z = p3_z - goal_z
# py = [y + start_y for y in py]
return pnt3D_xLst, pnt3D_yLst, pnt3D_zLst, dis_d, (goal_dir_x, goal_dir_y,
goal_dir_z)
def dubins_3Dpath_planning(start_x, start_y, start_z, start_psi, start_gamma,
end_x, end_y, end_z, end_psi, end_gamma, Rmin):
#
Cpsi_i_I, Spsi_i_I = cosine_sine(start_psi)
Cgamma_i_I, Sgamma_i_I = cosine_sine(start_gamma)
Cpsi_f_I, Spsi_f_I = cosine_sine(end_psi)
Cgamma_f_I, Sgamma_f_I = cosine_sine(end_gamma)
X_i_I = np.array([start_x, start_y, start_z])
X_f_I = np.array([end_x, end_y, end_z])
# Transformation matrix from Intertial to T-plane
ItoT = np.array(
[[Cgamma_i_I * Cpsi_i_I, Cgamma_i_I * Spsi_i_I, Sgamma_i_I],
[-Spsi_i_I, Cpsi_i_I, 0.0],
[-Sgamma_i_I * Cpsi_i_I, -Sgamma_i_I * Spsi_i_I, Cgamma_i_I]])
ev_f_I = np.array(
[Cgamma_f_I * Cpsi_f_I, Cgamma_f_I * Spsi_f_I, Sgamma_f_I])
ev_f_T = ItoT.dot(ev_f_I)
X_f_T = ItoT.dot(X_f_I - X_i_I)
psi_f_T, gamma_f_T = obtain_heading_fpa(ev_f_T)
# Path planning in T-plane
px_T, py_T, pz_T, ppsi_T, pgamma_T, clen_T, mode_T = Tplane_maneuver(
X_f_T, ev_f_T, psi_f_T, gamma_f_T, Rmin)
# Angle between T-plane and P-plane
X1_f_T = X_f_T + ev_f_T
X_t0_T = np.array([px_T[-1], py_T[-1], pz_T[-1]])
psi_t0_T = ppsi_T[-1]
Cpsi_t0_T, Spsi_t0_T = cosine_sine(psi_t0_T)
X1_t0_T = X_t0_T + np.array([Cpsi_t0_T, Spsi_t0_T, 0.0])
a = X1_f_T[2] * (X_f_T[1] - X_t0_T[1]) - X_f_T[2] * (X1_f_T[1] - X_t0_T[1])
b = X_f_T[2] * (X1_f_T[0] - X_t0_T[0]) - X1_f_T[2] * (X_f_T[0] - X_t0_T[0])
c = (X_f_T[0] - X_t0_T[0]) * (X1_f_T[1] - X_t0_T[1]) - (
X1_f_T[0] - X_t0_T[0]) * (X_f_T[1] - X_t0_T[1])
# Normal vectors of T plane and P plane
NT = np.array([0, 0, 1])
NP = np.array([a, b, c])
GAMMA = angle_between(NT, NP)
CGAMMA, SGAMMA = cosine_sine(GAMMA)
# Transformation matrix from T-plane to P-plane
TtoP = np.array([[Cpsi_t0_T, Spsi_t0_T, 0.0],
[-Spsi_t0_T * CGAMMA, Cpsi_t0_T * CGAMMA, SGAMMA],
[Spsi_t0_T * SGAMMA, -Cpsi_t0_T * SGAMMA, CGAMMA]])
X_f_P = TtoP.dot(X_f_T - X_t0_T)
Cpsi_f_T, Spsi_f_T = cosine_sine(psi_f_T)
Cgamma_f_T, Sgamma_f_T = cosine_sine(gamma_f_T)
ev_f_P = TtoP.dot(ev_f_T)
# print("-----In Dubins_3D_path_planning-----")
# print("ItoT = ",ItoT,"\n","X_f_T = ",X_f_T,"\n","ev_f_I = ",ev_f_I,"\n","ev_f_T = ",ev_f_T,"\n","psi_t0_T = ",psi_t0_T,"\n","X1_f_T = ",X1_f_T,"\n")
# print("psi_f_T = ",psi_f_T,"\n","gamma_f_T = ",gamma_f_T,"\n","X_t0_T = ",X_t0_T,"\n","X1_t0_T = ",X1_t0_T,"\n","a = ",a,"\n","b = ",b,"\n","c = ",c,"\n")
# print("GAMMA = ",GAMMA,"\n","TtoP = ",TtoP,"\n","X_f_P = ",X_f_P,"\n","ev_f_P = ",ev_f_P,"\n")
# Path Planning in P-plane
psi_f_P = math.atan2(ev_f_P[1], ev_f_P[0] + 1e-06)
px_P, py_P, ppsi_P, mode_P, clen_P = dubins_2Dpath_planning.dubins_path_planning(
0.0, 0.0, 0.0, X_f_P[0], X_f_P[1], psi_f_P, 1 / Rmin)
pz_P, pgamma_P = [0.0] * len(px_P), [0.0] * len(ppsi_P)
# Transform the path coordinates from P-plane to T-plane
px_Ttemp, py_Ttemp, pz_Ttemp = transform_trajectory(
px_P, py_P, pz_P, X_t0_T, TtoP.T)
ppsi_Ttemp, pgamma_Ttemp = transform_crossingdir(ppsi_P, pgamma_P, TtoP.T)
px_T, py_T, pz_T = px_T + px_Ttemp, py_T + py_Ttemp, pz_T + pz_Ttemp
ppsi_T, pgamma_T = ppsi_T + ppsi_Ttemp, pgamma_T + pgamma_Ttemp
# Transform the path coordinates from T-plane to inertial plane
px_I, py_I, pz_I = transform_trajectory(px_T, py_T, pz_T, X_i_I, ItoT.T)
ppsi_I, pgamma_I = transform_crossingdir(ppsi_T, pgamma_T, ItoT.T)
return px_I, py_I, pz_I, ppsi_I, pgamma_I, clen_T + clen_P, mode_T + mode_P
def in_gdubins3D(start_x, start_y, start_z, goal_x, goal_y, goal_z, rad,
mat2Dconvert3D, mat3Dconvert2D):
# print(goal_x,goal_y,goal_z)
p2_x, p2_y, p2_z = pnt3DConvert2Pnt2D(goal_x - start_x, goal_y - start_y,
goal_z - start_z, mat3Dconvert2D)
print('XXX PPP', float(p2_x), p2_y, p2_z)
# raise Exception('x')
# exit()
# wtf_x, wtf_y, wtf_z = pnt2DConvert2Pnt3D(p2_x, p2_x, mat2Dconvert3D)
#
# print(wtf_x + start_x,wtf_y + start_y,wtf_z + start_z)
# print('rad = ',rad)
# print('需要改半径')
print(rad)
px, py, pyaw, mode, clen = db.dubins_path_planning(0, 0, 0, p2_x, p2_y,
rad, 200)
dis_d = 0
for i in range(len(px) - 1):
dis_d += math.sqrt((px[i] - px[i + 1])**2 + (py[i] - py[i + 1])**2)
print(dis_d)
# print(np.linalg.norm([px,py]))
print(clen * 200)
# exit()
# print('inside_dis',dis_d)
# print(mode)
# print(dis_d)
pnt3D_xLst = []
pnt3D_yLst = []
pnt3D_zLst = []
for i in range(len(px)):
p3_x, p3_y, p3_z = pnt2DConvert2Pnt3D(px[i], py[i], mat2Dconvert3D)
pnt3D_xLst.append(p3_x)
pnt3D_yLst.append(p3_y)
pnt3D_zLst.append(p3_z)
pnt3D_xLst = [x + start_x for x in pnt3D_xLst]
pnt3D_yLst = [y + start_y for y in pnt3D_yLst]
pnt3D_zLst = [z + start_z for z in pnt3D_zLst]
# print(pyaw[-1])
p2_x_step = 2000 * cos(pyaw[-1]) + p2_x
p2_y_step = 2000 * sin(pyaw[-1]) + p2_y
# print(p2_x_step**2+ p2_y_step**2)
p3_x, p3_y, p3_z = pnt2DConvert2Pnt3D(p2_x_step, p2_y_step, mat2Dconvert3D)
# print('bfp3_x',p3_x)
# print('bfp3_y',p3_y)
# print('bfp3_z',p3_z)
p3_x = p3_x + start_x
p3_y = p3_y + start_y
p3_z = p3_z + start_z
# print('p3_x',p3_x)
# print('p3_y',p3_y)
# print('p3_z',p3_z)
bias_x = p3_x - goal_x
bias_y = p3_y - goal_y
bias_z = p3_z - goal_z
# print('b_x',bias_x)
# print('b_y',bias_y)
# print('b_z',bias_z)
# print('dis = ',distance([bias_x,bias_y,bias_z],[0,0,0]))
# bias_x = pnt3D_xLst[-1] - pnt3D_xLst[-2]
# bias_y = pnt3D_yLst[-1] - pnt3D_yLst[-2]
# bias_z = pnt3D_zLst[-1] - pnt3D_zLst[-2]
#
# print('b_x', bias_x)
# print('b_y', bias_y)
# print('b_z', bias_z)
#
# print('dis = ',distance([bias_x,bias_y,bias_z],[0,0,0]))
a_gama = math.atan(bias_z / 2000)
# if a_gama >= 0:
# print('a_gama >0')
# a_gama = a_gama
# else:
# print('a_gama <0')
# a_gama = math.pi + a_gama
if bias_x > 0 and bias_y > 0:
# print('case1')
a_psi = math.atan(bias_y / bias_x)
elif bias_x < 0 and bias_y <= 0:
# print('case2')
a_psi = math.atan(bias_y / bias_x) + math.pi
elif bias_x > 0 and bias_y <= 0:
# print('case3')
a_psi = math.atan(bias_y / bias_x)
else:
# print('case4')
a_psi = math.atan(bias_y / bias_x) + math.pi
return pnt3D_xLst, pnt3D_yLst, pnt3D_zLst, dis_d, a_psi, a_gama, p3_x, p3_y, p3_z