akai-transform 0.0.1__tar.gz

akai_transform-0.0.1/CLAUDE.md

@@ -0,0 +1,301 @@
+# akai_transform 编程指南
+
+纯 Python 空间变换工具库，提供 2D/3D 空间变换、四元数运算、SE(3) 李群/李代数、对数四元数等功能。仅依赖 `numpy`。
+
+---
+
+## 模块总览
+
+| 模块 | 导入方式 | 功能 |
+|---|---|---|
+| `tf2d` | `from akai_transform import tf2d` | 2D 空间变换（3×3 齐次矩阵） |
+| `tf3d` | `from akai_transform import tf3d` | 3D 空间变换（4×4 齐次矩阵） |
+| `quat` | `from akai_transform import quat` | 四元数运算 |
+| `se3` | `from akai_transform import se3` | SE(3) 李群/李代数 |
+| `log_quat` | `from akai_transform import log_quat` | 对数四元数 |
+| `const` | `from akai_transform import const` | 常量和枚举 |
+
+### 常量便捷导入
+
+```python
+from akai_transform import MM, M, DEG, RAD, PI, DEG2RAD, RAD2DEG
+```
+
+---
+
+## 约定
+
+- **欧拉角**：ZYX 内旋（RPY），`[roll, pitch, yaw]` = `[绕X, 绕Y, 绕Z]`
+- **角度单位**：默认弧度（rad），需要度时通过参数 `rpy_unit=DEG` 指定
+- **四元数**：`[w, x, y, z]` 标量在前
+- **2D 变换矩阵**：3×3 齐次矩阵
+- **3D 变换矩阵**：4×4 齐次矩阵
+- **平移单位**：通过 `MM`（毫米）或 `M`（米）枚举指定
+
+---
+
+## tf2d — 2D 空间变换
+
+```python
+from akai_transform import tf2d
+
+# 创建矩阵
+R = tf2d.rmat(np.pi / 4)              # 旋转 45°
+D = tf2d.dmat(100, 200)               # 平移 (100, 200)
+Dx = tf2d.dx(50)                      # 仅沿 X 轴平移
+Dy = tf2d.dy(30)                      # 仅沿 Y 轴平移
+T = tf2d.tmat(100, 200, np.pi / 4)    # 旋转 + 平移
+
+# 操作
+T_inv = tf2d.inv(T)                         # 利用正交性加速求逆
+v_a = tf2d.vec_transform(T, [10, 20])       # 坐标变换
+theta = tf2d.rmat2theta(R)                  # 提取角度
+
+# 由两向量求旋转
+theta = tf2d.theta_between_vecs([1,0], [0,1])  # 求旋转角度
+R = tf2d.rmat_between_vecs([1,0], [0,1])       # 求旋转矩阵
+
+# 近似相等判断
+tf2d.approx_equal_mat(T @ tf2d.inv(T), np.eye(3))  # True
+tf2d.approx_equal_vec([1.0, 2.0], [1.0, 2.0])      # True
+```
+
+---
+
+## tf3d — 3D 空间变换
+
+### 创建矩阵
+
+```python
+from akai_transform import tf3d
+
+R = tf3d.rmat(roll=0.1, pitch=0.2, yaw=0.3)  # ZYX 内旋
+Rx = tf3d.rx(0.1); Ry = tf3d.ry(0.2); Rz = tf3d.rz(0.3)
+D = tf3d.dmat(100, 200, 300)
+Dx = tf3d.dx(100); Dy = tf3d.dy(200); Dz = tf3d.dz(300)  # 单轴平移
+T = tf3d.tmat(100, 200, 300, 0.1, 0.2, 0.3)
+```
+
+### 变换操作
+
+```python
+T_inv = tf3d.inv(T)                           # 利用正交性加速求逆
+v_a = tf3d.vec_transform(T, [10, 20, 30])     # 坐标变换
+T_mm = tf3d.tmat_m2mm(T)                      # 米 → 毫米
+T_m = tf3d.tmat_mm2m(T_mm)                    # 毫米 → 米
+```
+
+### 旋转表示转换
+
+```python
+# 欧拉角 ↔ 旋转矩阵
+R = tf3d.euler2rmat(euler)
+euler = tf3d.rmat2euler(R)
+
+# 旋转向量 ↔ 旋转矩阵
+R = tf3d.rvec2rmat(n, theta)
+n, theta = tf3d.rmat2rvec(R)
+
+# 四元数 ↔ 旋转矩阵/欧拉角/旋转向量
+q = tf3d.rmat2quat(R);  R = tf3d.quat2rmat(q)
+q = tf3d.euler2quat(euler);  euler = tf3d.quat2euler(q)
+q = tf3d.rvec2quat(n, theta);  n, theta = tf3d.quat2rvec(q)
+
+# 修正罗德里格斯参数（MRP）
+mrp = tf3d.rmat2mrp(R);  R = tf3d.mrp2rmat(mrp)
+```
+
+### 由两向量求旋转
+
+```python
+v = [1, 0, 0]; v_dot = [0, 1, 0]
+n, theta = tf3d.rvec_between_vecs(v, v_dot)   # 旋转向量
+R = tf3d.rmat_between_vecs(v, v_dot)           # 旋转矩阵
+```
+
+### XYZRPY 便捷接口（机械臂常用）
+
+```python
+from akai_transform import tf3d, MM, M, DEG, RAD
+
+# 机械臂位姿 → 变换矩阵
+xyzrpy = [400, 0, 300, 180, 0, 0]  # mm + deg
+T = tf3d.xyzrpy2tmat(xyzrpy, xyz_unit=MM, rpy_unit=DEG, t_unit=MM)
+
+# 变换矩阵 → 机械臂位姿
+xyzrpy = tf3d.tmat2xyzrpy(T, t_unit=MM, xyz_unit=MM, rpy_unit=DEG)
+
+# 姿态增量变换（右乘增量）
+new_xyzrpy = tf3d.xyzrpy_transform(xyzrpy, x=10, y=0, z=0,
+                                     roll=0, pitch=0, yaw=5, rpy_unit=DEG)
+
+# XYZRPY 求逆
+inv_xyzrpy = tf3d.xyzrpy_inv(xyzrpy, rpy_unit=DEG)
+```
+
+### 高层工具
+
+```python
+T_mean = tf3d.tmat_mean([T1, T2, T3])           # 多个变换矩阵求均值
+R_clean = tf3d.orthogonalize_rmat(noisy_R)       # SVD 正交化
+dist, theta = tf3d.pose_error(T_real, T_est, is_debug=True)
+# 输出: 平移误差(mm) + 角度误差(deg)
+```
+
+---
+
+## quat — 四元数运算
+
+四元数格式统一为 `[w, x, y, z]`（标量在前）。
+
+```python
+from akai_transform import quat
+
+# 基础操作
+q_id = quat.identity()                   # [1, 0, 0, 0]
+n = quat.norm(q)                         # 模长
+ok = quat.is_unit(q)                     # 是否为单位四元数
+q_n = quat.normalize(q)                  # 归一化
+q_conj = quat.conjugate(q)               # 共轭 [w, -x, -y, -z]
+q_neg = quat.reverse(q)                  # 取反 [-w, -x, -y, -z]
+q_inv = quat.inv(q)                      # 求逆
+
+# 运算
+d = quat.dot(q1, q2)                     # 点乘
+q_prod = quat.multiply(q1, q2)           # Hamilton 积
+
+# 向量旋转
+v_rot = quat.rotate_vec(q, [1, 0, 0])           # 单个四元数旋转单个向量
+v_list = quat.rotate_vec_nq(q_list, [1, 0, 0])  # 多个四元数旋转同一向量
+v_list = quat.rotate_vec_nv(q, v_list)           # 同一四元数旋转多个向量
+
+# 度量
+theta, is_reversed = quat.angle_diff(q1, q2, select_min=True)
+q_delta = quat.relative_rotation(q1, q2)  # q_delta * q1 = q2
+
+# 插值
+q_lerp = quat.lerp(q1, q2, 0.5)         # 线性插值（结果非单位四元数）
+q_nlerp = quat.nlerp(q1, q2, 0.5)       # 正规化线性插值
+q_slerp = quat.slerp(q1, q2, 0.5)       # 球面线性插值（推荐）
+q_list = quat.interp_p2p(q1, q2, t_list, short_path=True)  # 批量插值
+
+# Squad 插值（多途径点平滑插值）
+q_interp = quat.squad(q0, q1, s0, s1, t)  # 单次 Squad 插值
+s = quat.compute_control_quat(q_prev, q_curr, q_next)  # 计算控制四元数
+
+# 多途径点插值（自动处理控制点）
+q_list_slerp = quat.interp_multi_waypoints(q_waypoints, t_list, method='slerp')  # 分段线性
+q_list_squad = quat.interp_multi_waypoints(q_waypoints, t_list, method='squad')  # 平滑过渡
+
+# 统计
+q_mean = quat.mean([q1, q2, q3, q4])     # 最大特征值法
+```
+
+---
+
+## se3 — SE(3) 李群/李代数
+
+```python
+from akai_transform import se3
+
+# hat/vee 算子
+omega_hat = se3.hat([1, 2, 3])        # R³ → so(3)
+omega = se3.vee(omega_hat)            # so(3) → R³
+
+# SO(3) 指数/对数映射
+R = se3.exp_so3([0.1, 0.2, 0.3])
+omega = se3.log_so3(R)
+
+# SE(3) 指数/对数映射
+T = se3.exp_se3(omega, v)
+omega, v = se3.log_se3(T)
+
+# 6维李代数 ↔ 变换矩阵
+xi = np.array([1, 2, 3, 0.1, 0.2, 0.3])  # [vx,vy,vz,wx,wy,wz]
+T = se3.xi2tmat(xi)
+xi = se3.tmat2xi(T)
+
+# SE(3) 螺旋插值
+T_mid = se3.interp_se3(T_start, T_end, t=0.5)
+```
+
+---
+
+## log_quat — 对数四元数
+
+```python
+from akai_transform import log_quat
+
+# 四元数 ↔ 对数空间
+v = log_quat.quat2log(q)        # 单位四元数 → R³
+q = log_quat.log2quat(v)        # R³ → 单位四元数
+
+# 支持批量处理 (N,4) → (N,3)
+v_batch = log_quat.quat2log(q_batch)
+q_batch = log_quat.log2quat(v_batch)
+```
+
+---
+
+## 常用代码模板
+
+### 机械臂位姿处理
+
+```python
+from akai_transform import tf3d, MM, DEG
+
+# 法奥机械臂 XYZRPY → 变换矩阵
+T = tf3d.xyzrpy2tmat([400, 0, 300, 180, 0, 0], xyz_unit=MM, rpy_unit=DEG, t_unit=MM)
+
+# 工具偏移
+T_tool = T @ tf3d.tmat(0, 0, 50, 0, 0, 0)
+
+# 逆变换
+T_inv = tf3d.inv(T_tool)
+```
+
+### 坐标系链式变换
+
+```python
+T_a2c = T_a2b @ T_b2c                     # A→B→C
+T_a2b = T_a2c @ tf3d.inv(T_b2c)           # 已知 A→C 和 B→C，求 A→B
+```
+
+### 姿态插值
+
+```python
+from akai_transform import tf3d, quat, se3
+
+# 四元数 Slerp 插值（两点间）
+q1 = tf3d.euler2quat([0, 0, 0])
+q2 = tf3d.euler2quat([0, 0, np.pi / 2])
+q_list = quat.interp_p2p(q1, q2, np.linspace(0, 1, 50))
+
+# 多途径点姿态插值
+q_waypoints = [tf3d.euler2quat(rpy) for rpy in rpy_list]
+t_interp = np.linspace(0, len(q_waypoints) - 1, 500)
+
+# 方法 1: Slerp（分段线性，类似位置空间的折线）
+q_list_slerp = quat.interp_multi_waypoints(q_waypoints, t_interp, method='slerp')
+
+# 方法 2: Squad（平滑过渡，类似位置空间的圆弧过渡）
+q_list_squad = quat.interp_multi_waypoints(q_waypoints, t_interp, method='squad')
+
+# SE(3) 螺旋插值（同时插值位置和姿态）
+T_list = [se3.interp_se3(T_start, T_end, t) for t in np.linspace(0, 1, 50)]
+```
+
+**Slerp vs Squad 对比**:
+
+| 特性 | Slerp | Squad |
+|------|-------|-------|
+| 连续性 | C0（位置连续） | C1（速度连续） |
+| 角速度 | 途径点处有突变 | 平滑连续 |
+| 类比 | 位置空间的折线 | 位置空间的圆弧过渡 |
+| 适用场景 | 简单路径、快速计算 | 高质量轨迹、减少机械冲击 |
+| 计算复杂度 | 低 | 中等 |
+
+**使用建议**:
+- 机械臂轨迹规划：优先使用 Squad，减少机械应力
+- 实时控制：Slerp 计算更快
+- 可视化动画：Squad 更平滑自然

akai_transform-0.0.1/MANIFEST.in

@@ -0,0 +1,5 @@
+include README.md
+include CLAUDE.md
+recursive-include akai_transform *.py
+recursive-include example *.py
+recursive-include image *

akai_transform-0.0.1/PKG-INFO

@@ -0,0 +1,127 @@
+Metadata-Version: 2.4
+Name: akai-transform
+Version: 0.0.1
+Summary: AKAI Spatial Transform Library
+Author-email: Shunkai Xing <xingshunkai@qq.com>
+Project-URL: Homepage, https://anyi.deepsenserobot.com/
+Keywords: spatial-transform,robotics,quaternion,euler-angles,se3,so3
+Classifier: Development Status :: 4 - Beta
+Classifier: Programming Language :: Python :: 3
+Classifier: Programming Language :: Python :: 3.8
+Classifier: Programming Language :: Python :: 3.9
+Classifier: Programming Language :: Python :: 3.10
+Classifier: Programming Language :: Python :: 3.11
+Classifier: Programming Language :: Python :: 3.12
+Classifier: Programming Language :: Python :: 3.13
+Classifier: Programming Language :: Python :: 3.14
+Classifier: Programming Language :: Python :: 3.15
+Classifier: Operating System :: OS Independent
+Classifier: Intended Audience :: Science/Research
+Classifier: Intended Audience :: Developers
+Classifier: Topic :: Scientific/Engineering :: Artificial Intelligence
+Classifier: Topic :: Scientific/Engineering
+Requires-Python: >=3.8
+Description-Content-Type: text/markdown
+Requires-Dist: numpy
+Provides-Extra: examples
+
+# 阿凯空间变换库(akai-transform)
+
+纯 Python 空间变换工具库，提供 2D/3D 空间变换、四元数运算、SE(3) 李群/李代数、对数四元数等功能。仅依赖 `numpy`。
+
+![](./image/阿凯空间变换库ICON-V2.png)
+
+## 文档
+
+- [阿凯爱玩机器人 课程官网](https://anyi.deepsenserobot.com/)
+- [理论基础-空间变换基础入门](https://anyi.deepsenserobot.com/course/akai-transform-course)
+- [阿凯空间变换库(akai-transform)使用手册](https://anyi.deepsenserobot.com/course/akai-transform-manual)
+
+![](./image/文档截图.png)
+
+
+
+## 安装
+
+```bash
+pip install akai-transform
+```
+
+
+
+## 模块总览
+
+| 模块 | 导入方式 | 功能 |
+|---|---|---|
+| `tf2d` | `from akai_transform import tf2d` | 2D 空间变换（3×3 齐次矩阵） |
+| `tf3d` | `from akai_transform import tf3d` | 3D 空间变换（4×4 齐次矩阵） |
+| `quat` | `from akai_transform import quat` | 四元数运算 |
+| `se3` | `from akai_transform import se3` | SE(3) 李群/李代数 |
+| `log_quat` | `from akai_transform import log_quat` | 对数四元数 |
+| `const` | `from akai_transform import const` | 常量和枚举 |
+
+
+
+## 约定
+
+- **欧拉角**：ZYX 内旋（RPY），`[roll, pitch, yaw]` = `[绕X, 绕Y, 绕Z]`
+- **角度单位**：默认弧度（rad），需要度时通过参数 `rpy_unit=DEG` 指定
+- **四元数**：`[w, x, y, z]` 标量在前
+- **2D 变换矩阵**：3×3 齐次矩阵
+- **3D 变换矩阵**：4×4 齐次矩阵
+- **平移单位**：通过 `MM`（毫米）或 `M`（米）枚举指定
+
+
+
+## 案例列表
+
+完整案例代码位于 `example/空间变换基础案例(akai_transform)/` 目录下。
+
+### 2D空间变换
+
+| 文件 | 说明 |
+|---|---|
+| `01_tf2d_基础测试.py` | tf2d 基础测试 |
+| `02_tf2d_小乌龟可视化.py` | 小乌龟可视化 |
+
+### 3D空间变换（基础）
+
+| 文件 | 说明 |
+|---|---|
+| `01_tf3d_旋转矩阵与平移矩阵.py` | 旋转矩阵与平移矩阵 |
+| `02_tf3d_旋转向量.py` | 旋转向量 |
+| `03_tf3d_四元数转换.py` | 四元数转换 |
+| `04_tf3d_XYZRPY.py` | XYZRPY 便捷接口 |
+| `05_tf3d_Open3D可视化.py` | Open3D 可视化 |
+
+### 3D空间变换（进阶）
+
+| 文件 | 说明 |
+|---|---|
+| `01_修正罗德里格斯参数(MRP).py` | 修正罗德里格斯参数 |
+| `02_四元数基础运算.py` | 四元数基础运算 |
+| `03_对数四元数转换.py` | 对数四元数转换 |
+| `04_多途径点位置姿态插值_Slerp_vs_Squad.py` | Slerp vs Squad 插值对比 |
+
+### 李群与李代数
+
+| 文件 | 说明 |
+|---|---|
+| `01_SE2公式推导(sympy).py` | SE2 公式推导（SymPy） |
+| `01_SE2指数映射与对数映射.py` | SE2 指数/对数映射 |
+| `02_SE3指数映射与对数映射.py` | SE3 指数/对数映射 |
+| `其他工具脚本/` | 三角函数可视化等辅助脚本 |
+
+## 作者
+
+**阿凯爱玩机器人**
+
+<img src="image/akai_3d_big.jpg" width="200" />
+
+扫码添加微信，一起交流机器人技术：
+
+<img src="image/akai_wechat_qrcode.png" width="200" />
+
+## 许可证
+
+MIT License

akai_transform-0.0.1/README.md

@@ -0,0 +1,100 @@
+# 阿凯空间变换库(akai-transform)
+
+纯 Python 空间变换工具库，提供 2D/3D 空间变换、四元数运算、SE(3) 李群/李代数、对数四元数等功能。仅依赖 `numpy`。
+
+![](./image/阿凯空间变换库ICON-V2.png)
+
+## 文档
+
+- [阿凯爱玩机器人 课程官网](https://anyi.deepsenserobot.com/)
+- [理论基础-空间变换基础入门](https://anyi.deepsenserobot.com/course/akai-transform-course)
+- [阿凯空间变换库(akai-transform)使用手册](https://anyi.deepsenserobot.com/course/akai-transform-manual)
+
+![](./image/文档截图.png)
+
+
+
+## 安装
+
+```bash
+pip install akai-transform
+```
+
+
+
+## 模块总览
+
+| 模块 | 导入方式 | 功能 |
+|---|---|---|
+| `tf2d` | `from akai_transform import tf2d` | 2D 空间变换（3×3 齐次矩阵） |
+| `tf3d` | `from akai_transform import tf3d` | 3D 空间变换（4×4 齐次矩阵） |
+| `quat` | `from akai_transform import quat` | 四元数运算 |
+| `se3` | `from akai_transform import se3` | SE(3) 李群/李代数 |
+| `log_quat` | `from akai_transform import log_quat` | 对数四元数 |
+| `const` | `from akai_transform import const` | 常量和枚举 |
+
+
+
+## 约定
+
+- **欧拉角**：ZYX 内旋（RPY），`[roll, pitch, yaw]` = `[绕X, 绕Y, 绕Z]`
+- **角度单位**：默认弧度（rad），需要度时通过参数 `rpy_unit=DEG` 指定
+- **四元数**：`[w, x, y, z]` 标量在前
+- **2D 变换矩阵**：3×3 齐次矩阵
+- **3D 变换矩阵**：4×4 齐次矩阵
+- **平移单位**：通过 `MM`（毫米）或 `M`（米）枚举指定
+
+
+
+## 案例列表
+
+完整案例代码位于 `example/空间变换基础案例(akai_transform)/` 目录下。
+
+### 2D空间变换
+
+| 文件 | 说明 |
+|---|---|
+| `01_tf2d_基础测试.py` | tf2d 基础测试 |
+| `02_tf2d_小乌龟可视化.py` | 小乌龟可视化 |
+
+### 3D空间变换（基础）
+
+| 文件 | 说明 |
+|---|---|
+| `01_tf3d_旋转矩阵与平移矩阵.py` | 旋转矩阵与平移矩阵 |
+| `02_tf3d_旋转向量.py` | 旋转向量 |
+| `03_tf3d_四元数转换.py` | 四元数转换 |
+| `04_tf3d_XYZRPY.py` | XYZRPY 便捷接口 |
+| `05_tf3d_Open3D可视化.py` | Open3D 可视化 |
+
+### 3D空间变换（进阶）
+
+| 文件 | 说明 |
+|---|---|
+| `01_修正罗德里格斯参数(MRP).py` | 修正罗德里格斯参数 |
+| `02_四元数基础运算.py` | 四元数基础运算 |
+| `03_对数四元数转换.py` | 对数四元数转换 |
+| `04_多途径点位置姿态插值_Slerp_vs_Squad.py` | Slerp vs Squad 插值对比 |
+
+### 李群与李代数
+
+| 文件 | 说明 |
+|---|---|
+| `01_SE2公式推导(sympy).py` | SE2 公式推导（SymPy） |
+| `01_SE2指数映射与对数映射.py` | SE2 指数/对数映射 |
+| `02_SE3指数映射与对数映射.py` | SE3 指数/对数映射 |
+| `其他工具脚本/` | 三角函数可视化等辅助脚本 |
+
+## 作者
+
+**阿凯爱玩机器人**
+
+<img src="image/akai_3d_big.jpg" width="200" />
+
+扫码添加微信，一起交流机器人技术：
+
+<img src="image/akai_wechat_qrcode.png" width="200" />
+
+## 许可证
+
+MIT License

akai_transform-0.0.1/akai_transform/__init__.py

@@ -0,0 +1,31 @@
+'''
+akai_transform — 空间变换工具库
+----------------------------------------------------------------
+提供 2D/3D 空间变换、四元数运算、SE(3) 李群/李代数、对数四元数等功能。
+
+子模块:
+- tf2d:     2D 空间变换（3×3 齐次变换矩阵）
+- tf3d:     3D 空间变换（4×4 齐次变换矩阵）
+- quat:     四元数运算（[w, x, y, z] 标量在前）
+- se3:      SE(3) 李群/李代数（指数/对数映射）
+- log_quat: 对数四元数（四元数 ↔ R³ 旋转向量）
+- const:    常量和枚举定义
+
+作者: 阿凯爱玩机器人 | 微信: xingshunkai  | QQ: 244561792
+官网: deepsenserobot.com
+B站: https://space.bilibili.com/40344504
+'''
+
+from . import const
+from . import tf2d
+from . import tf3d
+from . import quat
+from . import se3
+from . import log_quat
+
+# 常量便捷导出
+from .const import (
+    PI, DEG2RAD, RAD2DEG, PI_2, PI_D_2,
+    TranslationUnit, AngleUnit,
+    MM, M, DEG, RAD,
+)

akai_transform-0.0.1/akai_transform/const.py

@@ -0,0 +1,51 @@
+'''
+常量与枚举定义
+----------------------------------------------------------------
+提供空间变换库所需的数学常量和单位枚举。
+
+作者: 阿凯爱玩机器人 | 微信: xingshunkai  | QQ: 244561792
+官网: deepsenserobot.com
+B站: https://space.bilibili.com/40344504
+'''
+
+import math
+from enum import IntEnum
+
+# ============================================================
+# 数学常量
+# ============================================================
+
+PI = math.pi
+'''圆周率'''
+
+DEG2RAD = math.pi / 180.0
+'''角度转弧度系数'''
+
+RAD2DEG = 180.0 / math.pi
+'''弧度转角度系数'''
+
+PI_2 = math.pi * 2.0
+'''两倍圆周率 (2π)'''
+
+PI_D_2 = math.pi / 2.0
+'''圆周率的一半 (π/2)'''
+
+# ============================================================
+# 单位枚举
+# ============================================================
+
+class TranslationUnit(IntEnum):
+    '''平移量单位枚举'''
+    MM = 0  # 毫米
+    M = 1   # 米
+
+class AngleUnit(IntEnum):
+    '''角度单位枚举'''
+    DEG = 0  # 度
+    RAD = 1  # 弧度
+
+# 便捷别名
+MM = TranslationUnit.MM
+M = TranslationUnit.M
+DEG = AngleUnit.DEG
+RAD = AngleUnit.RAD

akai_transform-0.0.1/akai_transform/log_quat.py

@@ -0,0 +1,84 @@
+'''
+对数四元数模块
+----------------------------------------------------------------
+提供四元数与对数四元数（R³ 旋转向量）之间的映射，
+用于姿态插值和优化。
+
+理论基础:
+- 对数映射: SO(3) → so(3), q → log(q)
+- 指数映射: so(3) → SO(3), v → exp(v)
+- 应用场景: 姿态插值、轨迹规划、姿态优化等
+
+作者: 阿凯爱玩机器人 | 微信: xingshunkai  | QQ: 244561792
+官网: deepsenserobot.com
+B站: https://space.bilibili.com/40344504
+'''
+
+import numpy as np
+
+
+def quat2log(q):
+    '''单位四元数 → 对数四元数（R³ 旋转向量）
+
+    使用对数映射将 SO(3) 群映射到其李代数 so(3) ≅ R³。
+    支持单个或批量四元数输入。
+
+    :param q: 单位四元数 [w, x, y, z]，形状为 (4,) 或 (..., 4)
+    :return: 对数四元数(旋转向量) [vx, vy, vz]，形状为 (3,) 或 (..., 3)
+    '''
+    q = np.asarray(q, dtype=np.float64)
+    original_shape = q.shape
+
+    if q.ndim == 1:
+        q = q.reshape(1, -1)
+
+    # 归一化
+    q = q / np.linalg.norm(q, axis=-1, keepdims=True)
+
+    w = q[..., 0]
+    vec = q[..., 1:]
+    norm_vec = np.linalg.norm(vec, axis=-1)
+
+    log_q = np.zeros((*q.shape[:-1], 3), dtype=np.float64)
+
+    mask = norm_vec >= 1e-6
+    if np.any(mask):
+        theta = 2.0 * np.arctan2(norm_vec[mask], w[mask])
+        axis = vec[mask] / norm_vec[mask, np.newaxis]
+        log_q[mask] = axis * theta[:, np.newaxis]
+
+    if len(original_shape) == 1:
+        return log_q[0]
+    return log_q
+
+
+def log2quat(v):
+    '''对数四元数（R³ 旋转向量） → 单位四元数
+
+    使用指数映射将李代数 so(3) 映射回 SO(3) 群。
+    支持单个或批量输入。
+
+    :param v: 对数四元数(旋转向量) [vx, vy, vz]，形状为 (3,) 或 (..., 3)
+    :return: 单位四元数 [w, x, y, z]，形状为 (4,) 或 (..., 4)
+    '''
+    v = np.asarray(v, dtype=np.float64)
+    original_shape = v.shape
+
+    if v.ndim == 1:
+        v = v.reshape(1, -1)
+
+    theta = np.linalg.norm(v, axis=-1)
+
+    q = np.zeros((*v.shape[:-1], 4), dtype=np.float64)
+    q[..., 0] = 1.0
+
+    mask = theta >= 1e-6
+    if np.any(mask):
+        axis = v[mask] / theta[mask, np.newaxis]
+        half_theta = 0.5 * theta[mask]
+        q[mask, 0] = np.cos(half_theta)
+        q[mask, 1:] = axis * np.sin(half_theta)[:, np.newaxis]
+
+    if len(original_shape) == 1:
+        return q[0]
+    return q