robotengine.tools

  1import math
  2from typing import List, Tuple
  3
  4def hex2str(data) -> str:
  5    """
  6    将十六进制数据转换为字符串
  7
  8        :param data: 十六进制数据
  9        :return: 字符串
 10    """
 11    if isinstance(data, int):
 12        return f'{data:02X}'
 13    return ' '.join(f'{byte:02X}' for byte in data)
 14
 15def warning(msg) -> None:
 16    """
 17    打印警告信息
 18
 19        :param msg: 警告信息
 20    """
 21    msg = f"[WARNING] {msg}"
 22    print(f"\033[33m{msg}\033[0m")
 23
 24def error(msg) -> None:
 25    """
 26    打印错误信息,并退出程序
 27
 28        :param msg: 错误信息
 29    """
 30    msg = f"[ERROR] {msg}"
 31    print(f"\033[31m{msg}\033[0m")
 32    exit(1)
 33
 34def info(msg) -> None:
 35    """
 36    打印信息
 37
 38        :param msg: 信息内容
 39    """
 40
 41    msg = f"[INFO] {msg}"
 42    print(f"\033[32m{msg}\033[0m")
 43
 44def get_variable_name(obj) -> str:
 45    """
 46    获取变量名
 47
 48        :param obj: 变量对象
 49        :return: 变量名
 50    """
 51    for name, val in globals().items():
 52        if val is obj:
 53            return name
 54    return None
 55
 56def vector_length(x: float, y: float) -> float:
 57    """ 
 58    计算向量的长度 
 59    
 60        :param x: 向量的 x 分量
 61        :param y: 向量的 y 分量
 62        :return: 向量的长度
 63    """
 64    return math.sqrt(x * x + y * y)
 65
 66def vector_angle(x: float, y: float) -> float:
 67    """ 
 68    计算向量的角度(弧度制) 
 69    
 70        :param x: 向量的 x 分量
 71        :param y: 向量的 y 分量
 72        :return: 向量的角度(弧度制)
 73    """
 74    return math.atan2(y, x)
 75
 76def find_closest_vectors(angle: float, base_angles: List[float]) -> Tuple[int, int]:
 77    """ 
 78    判断角度在哪两个基向量之间 
 79    
 80        param angle: 角度(弧度制)
 81        param base_angles: 基向量的角度列表(弧度制)
 82        return: 两个基向量的索引
 83    """
 84    if angle < 0:
 85        angle += 2 * math.pi
 86    for i in range(4):
 87        next_i = (i + 1) % 4
 88        if base_angles[i] <= angle < base_angles[next_i]:
 89            return i, next_i
 90    # 如果在最边界情况下
 91    return 3, 0
 92
 93def near(a: float, b: float, threshold: float=1.0) -> bool:
 94    """ 
 95    判断两个浮点数是否接近 
 96    
 97        :param a: 第一个浮点数
 98        :param b: 第二个浮点数
 99        :param threshold: 阈值,默认为1.0
100        :return: 如果两个浮点数接近,则返回 True,否则返回 False
101    """
102    return abs(a - b) < threshold
103
104def compute_vector_projection(x, y, base_angles: List[float]) -> List[float]:
105    """
106    计算一个向量在一组基向量(数量为4)上的投影系数。假设我们有一个向量 (x, y) 和一组基向量 (basis1, basis2, basis3, basis4)。
107    我们的目标是找到一个线性组合,使得:
108
109        vector = coeff1 * basis1 + coeff2 * basis2 + coeff3 * basis3 + coeff4 * basis4
110
111    其中 coeff1、coeff2、coeff3 和 coeff4 是我们要求解的系数。
112    我们可以使用矩阵的方法来解决这个问题。假设我们有一个矩阵 A 和一个向量 b:
113
114        A = [basis1_x, basis1_y]
115            [basis2_x, basis2_y]
116            [basis3_x, basis3_y]
117            [basis4_x, basis4_y]
118
119        b = [x, y]
120
121    我们的目标是找到一个向量 coeffs,使得:
122
123        coeffs = A^-1 * b
124
125    但是由于矩阵 A 不一定可逆,我们可以使用矩阵的伪逆来代替:
126
127        coeffs = A^+ * b
128
129    其中 A^+ 是矩阵 A 的伪逆。
130
131    现在,我们已经得到了一个向量 coeffs,其中 coeffs[i] 表示向量 (x, y) 在第 i 个基向量上的投影系数。
132    
133    注意:
134        1. 此函数仅适用于基向量数量为4的情况。
135        2. 此函数仅适用于二维向量。
136        3. 此函数仅适用于基向量的角度为45度的情况。
137        4. 此函数仅适用于基向量的长度为1的情况。
138
139        :param x: 向量 x 分量
140        :param y: 向量 y 分量
141        :param base_angles: 基向量角度列表,长度为4
142        :return: 投影系数列表,长度为4
143    """
144
145    angle = vector_angle(x, y)
146    index1, index2 = find_closest_vectors(angle, base_angles)
147
148    # 计算两个基向量的坐标
149    basis1_x = math.cos(base_angles[index1])
150    basis1_y = math.sin(base_angles[index1])
151    basis2_x = math.cos(base_angles[index2])
152    basis2_y = math.sin(base_angles[index2])
153
154    # 构造线性方程 A * coeffs = b
155    A = [
156        [basis1_x, basis2_x],
157        [basis1_y, basis2_y]
158    ]
159    b = [x, y]
160
161    # 计算行列式
162    det = A[0][0] * A[1][1] - A[0][1] * A[1][0]
163
164    # 使用克拉梅尔法则求解系数
165    det1 = b[0] * A[1][1] - b[1] * A[0][1]
166    det2 = A[0][0] * b[1] - A[1][0] * b[0]
167
168    coeff1 = det1 / det
169    coeff2 = det2 / det
170
171    # 准备结果系数数组,长度为 4
172    coefficients = [0] * 4
173    coefficients[index1] = coeff1
174    coefficients[index2] = coeff2
175
176    return coefficients
def hex2str(data) -> str:
 5def hex2str(data) -> str:
 6    """
 7    将十六进制数据转换为字符串
 8
 9        :param data: 十六进制数据
10        :return: 字符串
11    """
12    if isinstance(data, int):
13        return f'{data:02X}'
14    return ' '.join(f'{byte:02X}' for byte in data)

将十六进制数据转换为字符串

:param data: 十六进制数据
:return: 字符串
def warning(msg) -> None:
16def warning(msg) -> None:
17    """
18    打印警告信息
19
20        :param msg: 警告信息
21    """
22    msg = f"[WARNING] {msg}"
23    print(f"\033[33m{msg}\033[0m")

打印警告信息

:param msg: 警告信息
def error(msg) -> None:
25def error(msg) -> None:
26    """
27    打印错误信息,并退出程序
28
29        :param msg: 错误信息
30    """
31    msg = f"[ERROR] {msg}"
32    print(f"\033[31m{msg}\033[0m")
33    exit(1)

打印错误信息,并退出程序

:param msg: 错误信息
def info(msg) -> None:
35def info(msg) -> None:
36    """
37    打印信息
38
39        :param msg: 信息内容
40    """
41
42    msg = f"[INFO] {msg}"
43    print(f"\033[32m{msg}\033[0m")

打印信息

:param msg: 信息内容
def get_variable_name(obj) -> str:
45def get_variable_name(obj) -> str:
46    """
47    获取变量名
48
49        :param obj: 变量对象
50        :return: 变量名
51    """
52    for name, val in globals().items():
53        if val is obj:
54            return name
55    return None

获取变量名

:param obj: 变量对象
:return: 变量名
def vector_length(x: float, y: float) -> float:
57def vector_length(x: float, y: float) -> float:
58    """ 
59    计算向量的长度 
60    
61        :param x: 向量的 x 分量
62        :param y: 向量的 y 分量
63        :return: 向量的长度
64    """
65    return math.sqrt(x * x + y * y)

计算向量的长度

:param x: 向量的 x 分量
:param y: 向量的 y 分量
:return: 向量的长度
def vector_angle(x: float, y: float) -> float:
67def vector_angle(x: float, y: float) -> float:
68    """ 
69    计算向量的角度(弧度制) 
70    
71        :param x: 向量的 x 分量
72        :param y: 向量的 y 分量
73        :return: 向量的角度(弧度制)
74    """
75    return math.atan2(y, x)

计算向量的角度(弧度制)

:param x: 向量的 x 分量
:param y: 向量的 y 分量
:return: 向量的角度(弧度制)
def find_closest_vectors(angle: float, base_angles: List[float]) -> Tuple[int, int]:
77def find_closest_vectors(angle: float, base_angles: List[float]) -> Tuple[int, int]:
78    """ 
79    判断角度在哪两个基向量之间 
80    
81        param angle: 角度(弧度制)
82        param base_angles: 基向量的角度列表(弧度制)
83        return: 两个基向量的索引
84    """
85    if angle < 0:
86        angle += 2 * math.pi
87    for i in range(4):
88        next_i = (i + 1) % 4
89        if base_angles[i] <= angle < base_angles[next_i]:
90            return i, next_i
91    # 如果在最边界情况下
92    return 3, 0

判断角度在哪两个基向量之间

param angle: 角度(弧度制)
param base_angles: 基向量的角度列表(弧度制)
return: 两个基向量的索引
def near(a: float, b: float, threshold: float = 1.0) -> bool:
 94def near(a: float, b: float, threshold: float=1.0) -> bool:
 95    """ 
 96    判断两个浮点数是否接近 
 97    
 98        :param a: 第一个浮点数
 99        :param b: 第二个浮点数
100        :param threshold: 阈值,默认为1.0
101        :return: 如果两个浮点数接近,则返回 True,否则返回 False
102    """
103    return abs(a - b) < threshold

判断两个浮点数是否接近

:param a: 第一个浮点数
:param b: 第二个浮点数
:param threshold: 阈值,默认为1.0
:return: 如果两个浮点数接近,则返回 True,否则返回 False
def compute_vector_projection(x, y, base_angles: List[float]) -> List[float]:
105def compute_vector_projection(x, y, base_angles: List[float]) -> List[float]:
106    """
107    计算一个向量在一组基向量(数量为4)上的投影系数。假设我们有一个向量 (x, y) 和一组基向量 (basis1, basis2, basis3, basis4)。
108    我们的目标是找到一个线性组合,使得:
109
110        vector = coeff1 * basis1 + coeff2 * basis2 + coeff3 * basis3 + coeff4 * basis4
111
112    其中 coeff1、coeff2、coeff3 和 coeff4 是我们要求解的系数。
113    我们可以使用矩阵的方法来解决这个问题。假设我们有一个矩阵 A 和一个向量 b:
114
115        A = [basis1_x, basis1_y]
116            [basis2_x, basis2_y]
117            [basis3_x, basis3_y]
118            [basis4_x, basis4_y]
119
120        b = [x, y]
121
122    我们的目标是找到一个向量 coeffs,使得:
123
124        coeffs = A^-1 * b
125
126    但是由于矩阵 A 不一定可逆,我们可以使用矩阵的伪逆来代替:
127
128        coeffs = A^+ * b
129
130    其中 A^+ 是矩阵 A 的伪逆。
131
132    现在,我们已经得到了一个向量 coeffs,其中 coeffs[i] 表示向量 (x, y) 在第 i 个基向量上的投影系数。
133    
134    注意:
135        1. 此函数仅适用于基向量数量为4的情况。
136        2. 此函数仅适用于二维向量。
137        3. 此函数仅适用于基向量的角度为45度的情况。
138        4. 此函数仅适用于基向量的长度为1的情况。
139
140        :param x: 向量 x 分量
141        :param y: 向量 y 分量
142        :param base_angles: 基向量角度列表,长度为4
143        :return: 投影系数列表,长度为4
144    """
145
146    angle = vector_angle(x, y)
147    index1, index2 = find_closest_vectors(angle, base_angles)
148
149    # 计算两个基向量的坐标
150    basis1_x = math.cos(base_angles[index1])
151    basis1_y = math.sin(base_angles[index1])
152    basis2_x = math.cos(base_angles[index2])
153    basis2_y = math.sin(base_angles[index2])
154
155    # 构造线性方程 A * coeffs = b
156    A = [
157        [basis1_x, basis2_x],
158        [basis1_y, basis2_y]
159    ]
160    b = [x, y]
161
162    # 计算行列式
163    det = A[0][0] * A[1][1] - A[0][1] * A[1][0]
164
165    # 使用克拉梅尔法则求解系数
166    det1 = b[0] * A[1][1] - b[1] * A[0][1]
167    det2 = A[0][0] * b[1] - A[1][0] * b[0]
168
169    coeff1 = det1 / det
170    coeff2 = det2 / det
171
172    # 准备结果系数数组,长度为 4
173    coefficients = [0] * 4
174    coefficients[index1] = coeff1
175    coefficients[index2] = coeff2
176
177    return coefficients

计算一个向量在一组基向量(数量为4)上的投影系数。假设我们有一个向量 (x, y) 和一组基向量 (basis1, basis2, basis3, basis4)。 我们的目标是找到一个线性组合,使得:

vector = coeff1 * basis1 + coeff2 * basis2 + coeff3 * basis3 + coeff4 * basis4

其中 coeff1、coeff2、coeff3 和 coeff4 是我们要求解的系数。 我们可以使用矩阵的方法来解决这个问题。假设我们有一个矩阵 A 和一个向量 b:

A = [basis1_x, basis1_y]
    [basis2_x, basis2_y]
    [basis3_x, basis3_y]
    [basis4_x, basis4_y]

b = [x, y]

我们的目标是找到一个向量 coeffs,使得:

coeffs = A^-1 * b

但是由于矩阵 A 不一定可逆,我们可以使用矩阵的伪逆来代替:

coeffs = A^+ * b

其中 A^+ 是矩阵 A 的伪逆。

现在,我们已经得到了一个向量 coeffs,其中 coeffs[i] 表示向量 (x, y) 在第 i 个基向量上的投影系数。

注意: 1. 此函数仅适用于基向量数量为4的情况。 2. 此函数仅适用于二维向量。 3. 此函数仅适用于基向量的角度为45度的情况。 4. 此函数仅适用于基向量的长度为1的情况。

:param x: 向量 x 分量
:param y: 向量 y 分量
:param base_angles: 基向量角度列表,长度为4
:return: 投影系数列表,长度为4