克隆后还缺仿真安装树和两边的编译目录。把 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 @@
+prepend-non-duplicate;AMENT_PREFIX_PATH;
@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
@@ -0,0 +1 @@
+prepend-non-duplicate;PYTHONPATH;lib/python3.11/site-packages
@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX/lib/python3.11/site-packages"
@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.11/site-packages"
@@ -0,0 +1,31 @@
+# generated from colcon_bash/shell/template/package.bash.em
+
+# This script extends the environment for this package.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  # the prefix is two levels up from the package specific share directory
+  _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
+else
+  _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_bash_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# source sh script of this package
+_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/arm_kinematics/package.sh"
+
+unset _colcon_package_bash_source_script
+unset _colcon_package_bash_COLCON_CURRENT_PREFIX
@@ -0,0 +1,6 @@
+source;share/arm_kinematics/hook/pythonpath.ps1
+source;share/arm_kinematics/hook/pythonpath.dsv
+source;share/arm_kinematics/hook/pythonpath.sh
+source;share/arm_kinematics/hook/ament_prefix_path.ps1
+source;share/arm_kinematics/hook/ament_prefix_path.dsv
+source;share/arm_kinematics/hook/ament_prefix_path.sh
@@ -0,0 +1,116 @@
+# generated from colcon_powershell/shell/template/package.ps1.em
+
+# function to append a value to a variable
+# which uses colons as separators
+# duplicates as well as leading separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_append_unique_value {
+  param (
+    $_listname,
+    $_value
+  )
+
+  # get values from variable
+  if (Test-Path Env:$_listname) {
+    $_values=(Get-Item env:$_listname).Value
+  } else {
+    $_values=""
+  }
+  $_duplicate=""
+  # start with no values
+  $_all_values=""
+  # iterate over existing values in the variable
+  if ($_values) {
+    $_values.Split(":") | ForEach {
+      # not an empty string
+      if ($_) {
+        # not a duplicate of _value
+        if ($_ -eq $_value) {
+          $_duplicate="1"
+        }
+        if ($_all_values) {
+          $_all_values="${_all_values}:$_"
+        } else {
+          $_all_values="$_"
+        }
+      }
+    }
+  }
+  # append only non-duplicates
+  if (!$_duplicate) {
+    # avoid leading separator
+    if ($_all_values) {
+      $_all_values="${_all_values}:${_value}"
+    } else {
+      $_all_values="${_value}"
+    }
+  }
+
+  # export the updated variable
+  Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_prepend_unique_value {
+  param (
+    $_listname,
+    $_value
+  )
+
+  # get values from variable
+  if (Test-Path Env:$_listname) {
+    $_values=(Get-Item env:$_listname).Value
+  } else {
+    $_values=""
+  }
+  # start with the new value
+  $_all_values="$_value"
+  # iterate over existing values in the variable
+  if ($_values) {
+    $_values.Split(":") | ForEach {
+      # not an empty string
+      if ($_) {
+        # not a duplicate of _value
+        if ($_ -ne $_value) {
+          # keep non-duplicate values
+          $_all_values="${_all_values}:$_"
+        }
+      }
+    }
+  }
+  # export the updated variable
+  Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+function colcon_package_source_powershell_script {
+  param (
+    $_colcon_package_source_powershell_script
+  )
+  # source script with conditional trace output
+  if (Test-Path $_colcon_package_source_powershell_script) {
+    if ($env:COLCON_TRACE) {
+      echo ". '$_colcon_package_source_powershell_script'"
+    }
+    . "$_colcon_package_source_powershell_script"
+  } else {
+    Write-Error "not found: '$_colcon_package_source_powershell_script'"
+  }
+}
+
+
+# a powershell script is able to determine its own path
+# the prefix is two levels up from the package specific share directory
+$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
+
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_kinematics/hook/pythonpath.ps1"
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_kinematics/hook/ament_prefix_path.ps1"
+
+Remove-Item Env:\COLCON_CURRENT_PREFIX
@@ -0,0 +1,87 @@
+# generated from colcon_core/shell/template/package.sh.em
+
+# This script extends the environment for this package.
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prepend_unique_value() {
+  # arguments
+  _listname="$1"
+  _value="$2"
+
+  # get values from variable
+  eval _values=\"\$$_listname\"
+  # backup the field separator
+  _colcon_prepend_unique_value_IFS=$IFS
+  IFS=":"
+  # start with the new value
+  _all_values="$_value"
+  # workaround SH_WORD_SPLIT not being set in zsh
+  if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
+    colcon_zsh_convert_to_array _values
+  fi
+  # iterate over existing values in the variable
+  for _item in $_values; do
+    # ignore empty strings
+    if [ -z "$_item" ]; then
+      continue
+    fi
+    # ignore duplicates of _value
+    if [ "$_item" = "$_value" ]; then
+      continue
+    fi
+    # keep non-duplicate values
+    _all_values="$_all_values:$_item"
+  done
+  unset _item
+  # restore the field separator
+  IFS=$_colcon_prepend_unique_value_IFS
+  unset _colcon_prepend_unique_value_IFS
+  # export the updated variable
+  eval export $_listname=\"$_all_values\"
+  unset _all_values
+  unset _values
+
+  unset _value
+  unset _listname
+}
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/cc/ros2/install_sim/arm_kinematics"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
+    echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+    unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+    return 1
+  fi
+  COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
+fi
+unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_sh_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# source sh hooks
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_kinematics/hook/pythonpath.sh"
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_kinematics/hook/ament_prefix_path.sh"
+
+unset _colcon_package_sh_source_script
+unset COLCON_CURRENT_PREFIX
+
+# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_kinematics</name>
+  <version>0.1.0</version>
+  <description>
+    纯几何 / 运动学工具：基于 TF 实测位形的几何雅可比与阻尼最小二乘逆解。
+    只依赖 numpy，不依赖 ROS —— 因此应用层（视觉伺服）与驱动层（柔顺下压）
+    都能复用它，而不会形成层次倒置。
+  </description>
+  <maintainer email="dev@example.com">arm_dev</maintainer>
+  <license>Apache-2.0</license>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
@@ -0,0 +1,42 @@
+# generated from colcon_zsh/shell/template/package.zsh.em
+
+# This script extends the environment for this package.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  # the prefix is two levels up from the package specific share directory
+  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
+else
+  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_zsh_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+colcon_zsh_convert_to_array() {
+  local _listname=$1
+  local _dollar="$"
+  local _split="{="
+  local _to_array="(\"$_dollar$_split$_listname}\")"
+  eval $_listname=$_to_array
+}
+
+# source sh script of this package
+_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/arm_kinematics/package.sh"
+unset convert_zsh_to_array
+
+unset _colcon_package_zsh_source_script
+unset _colcon_package_zsh_COLCON_CURRENT_PREFIX
@@ -0,0 +1 @@
+python3-numpy:python3-yaml
				`@@ -0,0 +1 @@`
				`prepend-non-duplicate;PYTHONPATH;lib/python3.11/site-packages`