ZYM/cumcm2024-code
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

write kinematics check

Browse Source
This commit is contained in:
ZhuangYumin
2024-09-07 23:16:53 +08:00
parent 095f63d154
commit 34250d6344
2 changed files with 207 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
A/4/loong.py
View File

@@ -16,7 +16,7 @@ class Orbit:
raise NotImplementedError
def C2Idx(self, C:[mp.mpf, mp.mpf]) -> mp.mpf:
raise NotImplementedError
def GenerateNextPointIdx(self, cur_point_idx:mp.mpf, expected_distance:mp.mpf)->mp.mpf:
def GenerateNextPointIdx(self, cur_point_idx:mp.mpf, expected_distance:mp.mpf, guess=None)->mp.mpf:
raise NotImplementedError
class Loong:
@@ -27,7 +27,7 @@ class Loong:
self.delta_idx = delta_idx
self.kSegLength1 = mp.mpf('2.86')
self.kSegLength2 = mp.mpf('1.65')
def CalcStatusListByIdx(self, cur_idx:mp.mpf):
def CalcStatusListByIdx(self, cur_idx:mp.mpf, last_time_status=None):
first_node_idx=cur_idx
first_node_C=self.orbit.Idx2C(first_node_idx)
first_node_dot = self.orbit.Idx2Cartesian(first_node_idx)
@@ -37,18 +37,18 @@ class Loong:
node_list = [{"idx": first_node_idx, "node": first_node_dot, "C": first_node_C, "v": self.speed}]
for i in range(1, self.total_points):
expected_distance = self.kSegLength1 if i == 1 else self.kSegLength2
cur_node_idx = self.orbit.GenerateNextPointIdx(node_list[-1]["idx"], expected_distance)
cur_node_idx = self.orbit.GenerateNextPointIdx(node_list[-1]["idx"], expected_distance, guess=last_time_status[i]["idx"] if last_time_status else None)
cur_node_dot = self.orbit.Idx2Cartesian(cur_node_idx)
cur_node_C = self.orbit.Idx2C(cur_node_idx)
virtual_cur_node_idx = self.orbit.GenerateNextPointIdx(virtual_first_node_idx, expected_distance)
virtual_cur_node_idx = self.orbit.GenerateNextPointIdx(virtual_first_node_idx, expected_distance, guess=last_time_status[i]["idx"] if last_time_status else None)
virtual_cur_node_C = self.orbit.Idx2C(virtual_cur_node_idx)
v = (virtual_cur_node_C - cur_node_C) / delta_T
node_list.append({"idx": cur_node_idx, "node": cur_node_dot, "C": cur_node_C, "v": v})
virtual_first_node_idx = virtual_cur_node_idx
return node_list
def CalcStatusListByTime(self, time_point:mp.mpf):
def CalcStatusListByTime(self, time_point:mp.mpf, last_time_status=None):
first_node_C = self.orbit.InitC() - time_point * self.speed
first_node_idx = self.orbit.C2Idx(first_node_C)
return self.CalcStatusListByIdx(first_node_idx)
return self.CalcStatusListByIdx(first_node_idx, last_time_status)