half finish A1

A/1/A1_to_csv.py

@@ -0,0 +1,5 @@
+import json
+with open("A1_res.json", "r") as file:
+  content=json.load(file)
+for lst in content:
+  print(lst[0])

A/1/cal.py

@@ -0,0 +1,103 @@
+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)

A/1/mp_cal.py

@@ -0,0 +1,79 @@
+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, 225):
+        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)