better write task1

A/1/A1_to_csv.py

@@ -2,13 +2,13 @@ import json
 with open("A1_res.json", "r") as file:
   content=json.load(file)
 fout1=open("tmp1.dat","w")
-for node_point in range(225):
+for node_point in range(224):
   for time_point in range(301):
     v=content[time_point][node_point]["v"]
     print(v,'\t',file=fout1,sep="",end="")
   print('\n',file=fout1,end="",sep="")
 fout2=open("tmp2.dat","w")
-for node_point in range(225):
+for node_point in range(224):
   for time_point in range(301):
     x=content[time_point][node_point]["node"][0]
     print(x,'\t',file=fout2,sep="",end="")

A/1/mp_cal.py

@@ -41,7 +41,7 @@ def CalcMoveList(time_point, last_theta):
 
     node_list = [{"theta": first_node_theta, "node": first_node_dot, "C": first_node_C, "v": mp.mpf('1.0')}]
 
-    for i in range(1, 225):
+    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)

A/1/mp_cal_it.py

@@ -0,0 +1,77 @@
+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)
+
+  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)
+    node_list.append({"theta": cur_node_theta, "node": cur_node_dot, "C": cur_node_C})
+  for i in range(223):
+    AA = kSegLength1 if i == 0 else kSegLength2
+    theta_i = node_list[i]["theta"]
+    theta_ip1 = node_list[i+1]["theta"]
+    alpha_i = mp.atan(theta_i)
+    alpha_ip1 = mp.atan(theta_ip1)
+    beta_i = mp.acos(((kAlpha*theta_i)**2 + AA**2 - (kAlpha*theta_ip1)**2) / (2*kAlpha*theta_i*AA))
+    gama_i = mp.acos(((kAlpha*theta_ip1)**2 + AA**2 - (kAlpha*theta_i)**2) / (2*kAlpha*theta_ip1*AA))
+    node_list[i+1]["v"] = node_list[i]["v"] * (-mp.cos(alpha_i + beta_i) / mp.cos(alpha_ip1 - gama_i))
+
+  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)