103 lines
3.3 KiB
Python
103 lines
3.3 KiB
Python
import sys
|
|
import json
|
|
import numba
|
|
import math
|
|
|
|
kSegLength1=2.86
|
|
kSegLength2=1.65
|
|
kAlpha=0.55/(2*math.pi)
|
|
if __name__ !="__main__":
|
|
sys.exit()
|
|
|
|
@numba.njit
|
|
def Theta2C(theta):
|
|
tmp=math.sqrt(1+theta*theta)
|
|
return kAlpha*0.5*(theta*tmp-math.log(-theta+tmp))
|
|
|
|
@numba.njit
|
|
def Theta2Dot(theta):
|
|
return (kAlpha*theta*math.cos(theta),kAlpha*theta*math.sin(theta))
|
|
res_list=[]
|
|
init_C=Theta2C(2*math.pi*16)
|
|
print(f"init_C={init_C}")
|
|
GenerateFirstNodeTheta_last_res_store=2*math.pi*16
|
|
@numba.njit
|
|
def GenerateFirstNodeTheta(time_point,GenerateFirstNodeTheta_last_res):
|
|
cur_C=init_C-1*time_point
|
|
R=GenerateFirstNodeTheta_last_res
|
|
L=GenerateFirstNodeTheta_last_res-10.0
|
|
if L<0:
|
|
L=0
|
|
M=(L+R)/2
|
|
kEps=1e-10
|
|
while True:
|
|
test_C=Theta2C(M)
|
|
# print(L,M,R,test_C-cur_C)
|
|
if -kEps < test_C-cur_C <kEps:
|
|
break
|
|
if test_C<cur_C:
|
|
L=M
|
|
M=(L+R)/2
|
|
elif test_C > cur_C:
|
|
R=M
|
|
M=(L+R)/2
|
|
GenerateFirstNodeTheta_last_res=M
|
|
return M
|
|
|
|
@numba.njit
|
|
def GenerateFollowNodeTheta(cur_node_theta,expected_distance):
|
|
cur_node_dot= Theta2Dot(cur_node_theta)
|
|
step=1.0
|
|
res_node_theta=cur_node_theta
|
|
kEps=1e-10
|
|
# print(f"expected_distance={expected_distance}")
|
|
while True:
|
|
test_node_theta=res_node_theta+step
|
|
test_node_dot=Theta2Dot(test_node_theta)
|
|
actual_distance=math.sqrt((cur_node_dot[0]-test_node_dot[0])**2 + (cur_node_dot[1]-test_node_dot[1])**2)
|
|
# print(f"actual_distance={actual_distance}, expected_distance={expected_distance}, step={step}")
|
|
if actual_distance < expected_distance-kEps:
|
|
res_node_theta+=step
|
|
elif actual_distance < expected_distance+kEps:
|
|
break
|
|
else:
|
|
pass
|
|
step*=0.5
|
|
return res_node_theta
|
|
|
|
def CalcMoveList(time_point):
|
|
global GenerateFirstNodeTheta_last_res_store
|
|
first_node_theta=GenerateFirstNodeTheta(time_point,GenerateFirstNodeTheta_last_res_store)
|
|
GenerateFirstNodeTheta_last_res_store=first_node_theta
|
|
first_node_dot=Theta2Dot(first_node_theta)
|
|
first_node_C=Theta2C(first_node_theta)
|
|
# print(f"\t{node_list}")
|
|
delta_theta=1e-4
|
|
virtual_first_node_theta=first_node_theta+delta_theta
|
|
virtual_first_node_C=Theta2C(virtual_first_node_theta)
|
|
virtual_lst_node_theta=virtual_first_node_theta
|
|
delta_T=virtual_first_node_C-first_node_C
|
|
node_list=[{"delta_theta":delta_theta,"delta_T":delta_T},{"theta":first_node_theta,"node":first_node_dot,"C":first_node_C,"v":1.0}]
|
|
for i in range(1,225):
|
|
if i==1:
|
|
expected_distance=kSegLength1
|
|
else:
|
|
expected_distance=kSegLength2
|
|
# print(f"i={i}, expected_distance={expected_distance}")
|
|
cur_node_theta=GenerateFollowNodeTheta(node_list[-1]["theta"],expected_distance)
|
|
cur_node_dot= Theta2Dot(cur_node_theta)
|
|
cur_node_C=Theta2C(cur_node_theta)
|
|
virtural_cur_node_theta=GenerateFollowNodeTheta(virtual_lst_node_theta,expected_distance)
|
|
virtural_cur_node_C=Theta2C(virtural_cur_node_theta)
|
|
v=(virtural_cur_node_C-cur_node_C)/delta_T
|
|
node_list.append({"theta":cur_node_theta,"node":cur_node_dot,"C":cur_node_C,"v":v})
|
|
virtual_lst_node_theta=virtural_cur_node_theta
|
|
# print(f"\t{node_list[-1]}")
|
|
return node_list
|
|
|
|
for time_point in range(0,301):
|
|
print(f"calculating time_point={time_point}")
|
|
res_list.append(CalcMoveList(time_point))
|
|
|
|
with open("A1_res.json","w") as file:
|
|
json.dump(res_list, file, indent=4) |