克隆后还缺仿真安装树和两边的编译目录。把 build、build_sim、install_sim 一并放进仓库；仿真入口不再写死本机 conda，build 里指向源码的链接改成相对路径。

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-10-15 11:07:04 +08:00
parent 926a7f9796
commit 80983954a4
5804 changed files with 2640781 additions and 3 deletions
@@ -0,0 +1,13 @@
+Metadata-Version: 2.4
+Name: arm_kinematics
+Version: 0.1.0
+Summary: 纯几何 / 运动学工具（几何雅可比 + DLS 逆解）
+Maintainer: arm_dev
+Maintainer-email: dev@example.com
+License: Apache-2.0
+Requires-Dist: setuptools
+Dynamic: license
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: requires-dist
+Dynamic: summary
@@ -0,0 +1,13 @@
+package.xml
+setup.cfg
+setup.py
+../../build_sim/arm_kinematics/arm_kinematics.egg-info/PKG-INFO
+../../build_sim/arm_kinematics/arm_kinematics.egg-info/SOURCES.txt
+../../build_sim/arm_kinematics/arm_kinematics.egg-info/dependency_links.txt
+../../build_sim/arm_kinematics/arm_kinematics.egg-info/requires.txt
+../../build_sim/arm_kinematics/arm_kinematics.egg-info/top_level.txt
+../../build_sim/arm_kinematics/arm_kinematics.egg-info/zip-safe
+arm_kinematics/__init__.py
+arm_kinematics/geom_ik.py
+resource/arm_kinematics
+test/test_geom_ik.py
@@ -0,0 +1 @@
+
@@ -0,0 +1 @@
+setuptools
@@ -0,0 +1 @@
+arm_kinematics
@@ -0,0 +1 @@
+
@@ -0,0 +1,33 @@
+"""纯几何 / 运动学工具（只依赖 numpy，不依赖 ROS）。
+
+目前提供基于 TF 实测位形的几何雅可比与阻尼最小二乘逆解，
+见 :mod:`arm_kinematics.geom_ik`。
+
+单独成包的动机：这套东西同时被应用层（视觉伺服、搜孔装配）和驱动层
+（柔顺下压）需要。原先它放在 ``arm_application`` 里，导致驱动层要反过来依赖
+应用层 —— 层次倒置。提取到这里之后两边都依赖一个无 ROS 依赖的底层包。
+"""
+
+from .geom_ik import (
+    RIZON_LINK_AXES,
+    dls_position_step,
+    dls_translate_hold_ori,
+    dls_twist,
+    geometric_jacobian,
+    load_handeye_T,
+    position_jacobian,
+    quat_xyzw_to_R,
+)
+
+__version__ = "0.1.0"
+
+__all__ = [
+    "RIZON_LINK_AXES",
+    "quat_xyzw_to_R",
+    "geometric_jacobian",
+    "position_jacobian",
+    "dls_position_step",
+    "dls_twist",
+    "dls_translate_hold_ori",
+    "load_handeye_T",
+]
@@ -0,0 +1,186 @@
+#!/usr/bin/env python3
+"""基于 TF 实测位形的几何雅可比 + 阻尼最小二乘（DLS）位置级逆解。
+
+为什么不需要 URDF / 正运动学
+---------------------------
+本模块做的是**速度级**逆解，不是位置级解析逆解：
+
+1. 从 TF 直接读每个关节 link 的当前位姿（``link_R`` / ``link_p``）
+2. 用固定的关节轴（``RIZON_LINK_AXES``，来自 Rizon4 URDF）构建几何雅可比 J
+3. 解 ``J dq = dp`` 得到小步 ``dq``，积分即可
+
+雅可比只是**当前位形**的函数，所以用 TF 实测位形算它是正确且实时的；不需要
+"给定关节角预测末端位姿"的正运动学。代价是只能走小步（大位移要迭代多次），
+而这恰好符合装配场景的需求 —— 每次挪 1mm 以内，边动边看力反馈。
+
+依赖：只依赖 numpy。不 import ROS，因此可被驱动层与测试直接复用。
+
+坐标系约定：所有输入输出都在**基座系**，末端位置 ``ee_p`` 与各 link 位置
+``link_p`` 必须同一参考系。
+"""
+
+from __future__ import annotations
+
+from typing import List, Optional, Sequence, Tuple
+
+import numpy as np
+
+#: Rizon4 各关节轴在**子 link 系**下的表达（来自 URDF）。
+#: 顺序对应 joint1..joint7 —— 相邻关节轴交替，标准 7 自由度布局。
+RIZON_LINK_AXES = [
+    (0.0, 0.0, 1.0),  # joint1 -> link1
+    (0.0, 1.0, 0.0),  # joint2 -> link2
+    (0.0, 0.0, 1.0),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+]
+
+
+def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray:
+    """四元数 (x, y, z, w) 转 3x3 旋转矩阵。自动归一化，零四元数也不炸。"""
+    x, y, z, w = [float(v) for v in q]
+    n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12
+    x, y, z, w = x / n, y / n, z / n, w / n
+    return np.array(
+        [
+            [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+            [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
+            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
+        ],
+        dtype=float,
+    )
+
+
+def geometric_jacobian(
+    link_R: Sequence[np.ndarray],
+    link_p: Sequence[np.ndarray],
+    ee_p: np.ndarray,
+    axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
+) -> np.ndarray:
+    """构建 6x7 基座系几何雅可比。前 3 行是线速度，后 3 行是角速度。
+
+    参数
+    ----
+    link_R:
+        各关节 link 在基座系下的 3x3 姿态，长度 = 关节数。
+    link_p:
+        各关节 link 在基座系下的 3 维位置。
+    ee_p:
+        末端（法兰）在基座系下的 3 维位置。
+    """
+    n = len(axes_local)
+    J = np.zeros((6, n), dtype=float)
+    pee = np.asarray(ee_p, dtype=float).reshape(3)
+    for i in range(n):
+        R = np.asarray(link_R[i], dtype=float).reshape(3, 3)
+        p = np.asarray(link_p[i], dtype=float).reshape(3)
+        z = R @ np.asarray(axes_local[i], dtype=float).reshape(3)
+        # 平动部分：关节轴 × (末端 − 关节点)；转动部分：关节轴本身
+        J[:3, i] = np.cross(z, pee - p)
+        J[3:, i] = z
+    return J
+
+
+def position_jacobian(
+    link_R: Sequence[np.ndarray],
+    link_p: Sequence[np.ndarray],
+    ee_p: np.ndarray,
+    axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
+) -> np.ndarray:
+    """只取几何雅可比的平动部分（3x7）。"""
+    return geometric_jacobian(link_R, link_p, ee_p, axes_local)[:3, :]
+
+
+def dls_position_step(
+    J: np.ndarray,
+    dp_base: np.ndarray,
+    *,
+    damping: float = 0.05,
+    max_dq: float = 0.05,
+) -> np.ndarray:
+    """解 ``J dq = dp``（阻尼最小二乘），并把 ``|dq|`` 逐元素限幅。
+
+    阻尼项 ``λ²I`` 的作用：接近奇异位形时避免关节速度爆掉。代价是有跟踪误差，
+    但对小幅步进（毫米级）可以忽略。
+    """
+    dp = np.asarray(dp_base, dtype=float).reshape(3)
+    JT = J.T
+    A = J @ JT + (damping ** 2) * np.eye(3)
+    dq = JT @ np.linalg.solve(A, dp)
+    dq = np.clip(dq, -abs(max_dq), abs(max_dq))
+    return dq
+
+
+def dls_twist(
+    J6: np.ndarray,
+    v_base: np.ndarray,
+    *,
+    w_base: Optional[np.ndarray] = None,
+    damping: float = 0.08,
+    max_qdot: float = 0.80,
+    ori_weight: float = 1.2,
+) -> np.ndarray:
+    """解 ``J qdot = [v; w]``，默认把角速度压到 0（保持姿态）。
+
+    ``ori_weight`` 给姿态分量额外的权重，让姿态在数值上更"硬"，减少平移时
+    末端姿态的漂移。
+    """
+    dx = np.zeros(6, dtype=float)
+    dx[:3] = np.asarray(v_base, dtype=float).reshape(3)
+    if w_base is not None:
+        dx[3:] = np.asarray(w_base, dtype=float).reshape(3)
+    w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
+    Jw = J6 * w[:, None]
+    dxw = dx * w
+    JT = Jw.T
+    A = Jw @ JT + (damping ** 2) * np.eye(6)
+    qdot = JT @ np.linalg.solve(A, dxw)
+    return np.clip(qdot, -abs(max_qdot), abs(max_qdot))
+
+
+def dls_translate_hold_ori(
+    J6: np.ndarray,
+    dp_base: np.ndarray,
+    *,
+    damping: float = 0.08,
+    max_dq: float = 0.05,
+    ori_weight: float = 1.2,
+) -> np.ndarray:
+    """平移末端 ``dp`` 同时保持姿态。"""
+    dx = np.zeros(6, dtype=float)
+    dx[:3] = np.asarray(dp_base, dtype=float).reshape(3)
+    w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
+    Jw = J6 * w[:, None]
+    dxw = dx * w
+    JT = Jw.T
+    A = Jw @ JT + (damping ** 2) * np.eye(6)
+    dq = JT @ np.linalg.solve(A, dxw)
+    return np.clip(dq, -abs(max_dq), abs(max_dq))
+
+
+def load_handeye_T(path: str) -> np.ndarray:
+    """从手眼结果 YAML 里读 4x4 齐次变换矩阵（键名 ``T_4x4``）。"""
+    import yaml
+
+    with open(path, "r", encoding="utf-8") as f:
+        data = yaml.safe_load(f)
+    if not isinstance(data, dict) or "T_4x4" not in data:
+        raise ValueError(f"No T_4x4 in hand-eye file: {path}")
+    T = np.array(data["T_4x4"], dtype=float)
+    if T.shape != (4, 4):
+        raise ValueError(f"T_4x4 shape {T.shape}, expected (4,4)")
+    return T
+
+
+__all__ = [
+    "RIZON_LINK_AXES",
+    "quat_xyzw_to_R",
+    "geometric_jacobian",
+    "position_jacobian",
+    "dls_position_step",
+    "dls_twist",
+    "dls_translate_hold_ori",
+    "load_handeye_T",
+]
@@ -0,0 +1 @@
+0
@@ -0,0 +1 @@
+# generated from colcon_core/shell/template/command_prefix.sh.em
@@ -0,0 +1 @@
+0
@@ -0,0 +1,12 @@
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics/geom_ik.py
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics/__init__.py
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics/__pycache__/geom_ik.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics/__pycache__/__init__.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_kinematics/share/ament_index/resource_index/packages/arm_kinematics
+/home/cc/ros2/install_sim/arm_kinematics/share/arm_kinematics/package.xml
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics-0.1.0-py3.11.egg-info/SOURCES.txt
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics-0.1.0-py3.11.egg-info/zip-safe
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics-0.1.0-py3.11.egg-info/top_level.txt
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics-0.1.0-py3.11.egg-info/dependency_links.txt
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics-0.1.0-py3.11.egg-info/PKG-INFO
+/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages/arm_kinematics-0.1.0-py3.11.egg-info/requires.txt
@@ -0,0 +1,4 @@
+import sys
+if sys.prefix == '/home/cc/anaconda3/envs/ros_gz_sim':
+    sys.real_prefix = sys.prefix
+    sys.prefix = sys.exec_prefix = '/home/cc/ros2/install_sim/arm_kinematics'
				`@@ -0,0 +1 @@`
				`# generated from colcon_core/shell/template/command_prefix.sh.em`