79 lines
2.8 KiB
Python
79 lines
2.8 KiB
Python
import mpmath as mp
|
|
import json
|
|
|
|
mp.dps = 15 # 设置精度为15位小数
|
|
|
|
kSegLength1 = mp.mpf('2.86')
|
|
kSegLength2 = mp.mpf('1.65')
|
|
kAlpha = mp.mpf('0.55') / (2 * mp.pi)
|
|
|
|
def Theta2C(theta):
|
|
tmp = mp.sqrt(1 + theta**2)
|
|
return kAlpha * 0.5 * (theta * tmp - mp.log(-theta + tmp))
|
|
|
|
def Theta2Dot(theta):
|
|
return (kAlpha * theta * mp.cos(theta), kAlpha * theta * mp.sin(theta))
|
|
|
|
init_C=Theta2C(2*mp.pi*16)
|
|
def GenerateFirstNodeTheta(time_point, last_theta):
|
|
cur_C = init_C - time_point
|
|
def f(theta):
|
|
return Theta2C(theta) - cur_C
|
|
return mp.findroot(f, last_theta, solver='secant')
|
|
|
|
def GenerateFollowNodeTheta(cur_node_theta, expected_distance):
|
|
cur_node_dot = Theta2Dot(cur_node_theta)
|
|
def f(theta):
|
|
test_node_dot = Theta2Dot(theta)
|
|
actual_distance = mp.sqrt((cur_node_dot[0]-test_node_dot[0])**2 + (cur_node_dot[1]-test_node_dot[1])**2)
|
|
return actual_distance - expected_distance
|
|
return mp.findroot(f, cur_node_theta + 0.1, solver='secant')
|
|
|
|
def CalcMoveList(time_point, last_theta):
|
|
first_node_theta = GenerateFirstNodeTheta(time_point, last_theta)
|
|
first_node_dot = Theta2Dot(first_node_theta)
|
|
first_node_C = Theta2C(first_node_theta)
|
|
|
|
delta_theta = mp.mpf('1e-8')
|
|
virtual_first_node_theta = first_node_theta + delta_theta
|
|
virtual_first_node_C = Theta2C(virtual_first_node_theta)
|
|
delta_T = virtual_first_node_C - first_node_C
|
|
|
|
node_list = [{"theta": first_node_theta, "node": first_node_dot, "C": first_node_C, "v": mp.mpf('1.0')}]
|
|
|
|
for i in range(1, 224):
|
|
expected_distance = kSegLength1 if i == 1 else kSegLength2
|
|
cur_node_theta = GenerateFollowNodeTheta(node_list[-1]["theta"], expected_distance)
|
|
cur_node_dot = Theta2Dot(cur_node_theta)
|
|
cur_node_C = Theta2C(cur_node_theta)
|
|
|
|
virtual_cur_node_theta = GenerateFollowNodeTheta(virtual_first_node_theta, expected_distance)
|
|
virtual_cur_node_C = Theta2C(virtual_cur_node_theta)
|
|
v = (virtual_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_first_node_theta = virtual_cur_node_theta
|
|
|
|
return node_list, first_node_theta
|
|
|
|
res_list = []
|
|
last_theta = 2 * mp.pi * 16
|
|
|
|
for time_point in range(301):
|
|
print(f"calculating time_point={time_point}")
|
|
move_list, last_theta = CalcMoveList(time_point, last_theta)
|
|
res_list.append(move_list)
|
|
|
|
# 将结果转换为float并保留6位小数
|
|
float_res_list = [
|
|
[
|
|
{k: round(float(v), 6) if isinstance(v, mp.mpf) else
|
|
[round(float(x), 6) for x in v] if isinstance(v, tuple) else v
|
|
for k, v in node.items()}
|
|
for node in time_point
|
|
]
|
|
for time_point in res_list
|
|
]
|
|
|
|
with open("A1_res.json", "w") as file:
|
|
json.dump(float_res_list, file, indent=4) |