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