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

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-15 11:07:04 +08:00
parent 926a7f9796
commit 80983954a4
5804 changed files with 2640781 additions and 3 deletions
+1
View File
@@ -0,0 +1 @@
../../src/arm_kinematics/arm_kinematics
@@ -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,18 @@
package.xml
setup.cfg
setup.py
../../build/arm_kinematics/arm_kinematics.egg-info/PKG-INFO
../../build/arm_kinematics/arm_kinematics.egg-info/SOURCES.txt
../../build/arm_kinematics/arm_kinematics.egg-info/dependency_links.txt
../../build/arm_kinematics/arm_kinematics.egg-info/requires.txt
../../build/arm_kinematics/arm_kinematics.egg-info/top_level.txt
../../build/arm_kinematics/arm_kinematics.egg-info/zip-safe
arm_kinematics/__init__.py
arm_kinematics/geom_ik.py
arm_kinematics.egg-info/PKG-INFO
arm_kinematics.egg-info/SOURCES.txt
arm_kinematics.egg-info/dependency_links.txt
arm_kinematics.egg-info/requires.txt
arm_kinematics.egg-info/top_level.txt
arm_kinematics.egg-info/zip-safe
resource/arm_kinematics
@@ -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",
]
+1
View File
@@ -0,0 +1 @@
0
@@ -0,0 +1 @@
# generated from colcon_core/shell/template/command_prefix.sh.em
@@ -0,0 +1,132 @@
AMENT_PREFIX_PATH=/home/cc/ros2/install/gpio_controller:/home/cc/ros2/install/flexiv_bringup:/home/cc/ros2/install/flexiv_test_nodes:/home/cc/ros2/install/flexiv_robot_states_broadcaster:/home/cc/ros2/install/flexiv_gripper:/home/cc/ros2/install/flexiv_msgs:/home/cc/ros2/install/flexiv_moveit_config:/home/cc/ros2/install/flexiv_hardware:/home/cc/ros2/install/arm_bringup:/home/cc/ros2/install/arm_moveit_config:/home/cc/ros2/install/arm_description:/home/cc/ros2/install/flexiv_description:/home/cc/ros2/install/arm_driver_flexiv:/home/cc/ros2/install/arm_control:/home/cc/ros2/install/arm_application:/home/cc/ros2/install/arm_msgs:/home/cc/ros2/install/arm_kinematics:/home/cc/ros2/install/arm_interfaces:/home/cc/anaconda3/envs/ros_flexiv
CMAKE_PREFIX_PATH=/home/cc/ros2/install/gpio_controller:/home/cc/ros2/install/flexiv_bringup:/home/cc/ros2/install/flexiv_robot_states_broadcaster:/home/cc/ros2/install/flexiv_gripper:/home/cc/ros2/install/flexiv_msgs:/home/cc/ros2/install/flexiv_moveit_config:/home/cc/ros2/install/flexiv_hardware:/home/cc/ros2/install/arm_bringup:/home/cc/ros2/install/arm_moveit_config:/home/cc/ros2/install/arm_description:/home/cc/ros2/install/flexiv_description:/home/cc/ros2/install/arm_msgs
COLCON=1
COLCON_PREFIX_PATH=/home/cc/ros2/install
CONDA_DEFAULT_ENV=ros_flexiv
CONDA_EXE=/home/cc/anaconda3/bin/conda
CONDA_PREFIX=/home/cc/anaconda3/envs/ros_flexiv
CONDA_PREFIX_1=/home/cc/anaconda3/envs/ros_gz_sim
CONDA_PROMPT_MODIFIER=(ros_flexiv)
CONDA_PYTHON_EXE=/home/cc/anaconda3/bin/python
CONDA_SHLVL=2
GSETTINGS_SCHEMA_DIR=/home/cc/anaconda3/envs/ros_flexiv/share/glib-2.0/schemas
GSETTINGS_SCHEMA_DIR_CONDA_BACKUP=
HOME=/home/cc
LC_CTYPE=C.UTF-8
LD_LIBRARY_PATH=/home/cc/anaconda3/envs/ros_flexiv/lib/python3.11/site-packages/tensorrt_libs:/home/cc/anaconda3/envs/ros_flexiv/lib/python3.11/site-packages/tensorrt:/home/cc/flexiv_install/lib:/home/cc/ros2/install/gpio_controller/lib:/home/cc/ros2/install/flexiv_robot_states_broadcaster/lib:/home/cc/ros2/install/flexiv_gripper/lib:/home/cc/ros2/install/flexiv_msgs/lib:/home/cc/ros2/install/flexiv_hardware/lib:/home/cc/ros2/install/arm_msgs/lib:/home/cc/anaconda3/envs/ros_flexiv/opt/rviz_ogre_vendor/lib:/home/cc/ros2/install_sim/arm_msgs/lib:/home/cc/anaconda3/envs/ros_gz_sim/opt/rviz_ogre_vendor/lib
OLDPWD=/home/cc/ros2
PATH=/home/cc/anaconda3/envs/ros_gz_sim/bin:/home/cc/anaconda3/envs/ros_gz_sim/share/rubygems/bin:/home/cc/anaconda3/envs/ros_flexiv/bin:/home/cc/anaconda3/condabin:/usr/bin:/bin:/usr/sbin:/sbin
PWD=/home/cc/ros2/build/arm_kinematics
PYTHONPATH=/home/cc/ros2/build/flexiv_test_nodes:/home/cc/ros2/install/flexiv_test_nodes/lib/python3.11/site-packages:/home/cc/ros2/install/flexiv_msgs/lib/python3.11/site-packages:/home/cc/ros2/build/arm_driver_flexiv:/home/cc/ros2/install/arm_driver_flexiv/lib/python3.11/site-packages:/home/cc/ros2/build/arm_control:/home/cc/ros2/install/arm_control/lib/python3.11/site-packages:/home/cc/ros2/build/arm_application:/home/cc/ros2/install/arm_application/lib/python3.11/site-packages:/home/cc/ros2/install/arm_msgs/lib/python3.11/site-packages:/home/cc/ros2/build/arm_kinematics:/home/cc/ros2/install/arm_kinematics/lib/python3.11/site-packages:/home/cc/ros2/build/arm_interfaces:/home/cc/ros2/install/arm_interfaces/lib/python3.11/site-packages:/home/cc/anaconda3/envs/ros_flexiv/lib/python311/site-packages:/home/cc/anaconda3/envs/ros_flexiv/lib/python3.11/site-packages:/home/cc/ros2/install_sim/arm_control/lib/python3.11/site-packages:/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages:/home/cc/ros2/install_sim/arm_msgs/lib/python3.11/site-packages:/home/cc/ros2/install_sim/arm_kinematics/lib/python3.11/site-packages:/home/cc/ros2/install_sim/arm_interfaces/lib/python3.11/site-packages:/home/cc/anaconda3/envs/ros_gz_sim/lib/python311/site-packages:/home/cc/anaconda3/envs/ros_gz_sim/lib/python3.11/site-packages
ROBOT_SN=Rizon4-063989
ROS2_WS=/home/cc/ros2
ROS_DISTRO=humble
ROS_DOMAIN_ID=77
ROS_ETC_DIR=/home/cc/anaconda3/envs/ros_flexiv/etc/ros
ROS_LOCALHOST_ONLY=0
ROS_OS_OVERRIDE=conda:linux
ROS_PYTHON_VERSION=3
ROS_VERSION=2
SHLVL=0
SITE_BASE_FRAME=Rizon4-063989_base_link
SITE_CAMERA_FRAME=d405_left_color_optical_frame
SITE_CAM_AE_LIMIT_US=4000.0
SITE_CAM_AUTO_EXPOSURE=1
SITE_CAM_EMITTER=0
SITE_CAM_ENABLE_DEPTH=1
SITE_CAM_ENABLE_INFRA=1
SITE_CAM_EXPOSURE_US=0.0
SITE_CAM_FPS=30
SITE_CAM_GAIN=0.0
SITE_CAM_HEIGHT=480
SITE_CAM_LASER_POWER=0.0
SITE_CAM_NAME=d405_left
SITE_CAM_OVERLAY_HOLD=0.1
SITE_CAM_PREVIEW=1
SITE_CAM_PREVIEW_STEREO=1
SITE_CAM_SERIAL=352122274408
SITE_CAM_USE_SITE_RES=1
SITE_CAM_WIDTH=848
SITE_CONFIG=/home/cc/ros2/config/site.yaml
SITE_D405_LEFT_SERIAL=352122274408
SITE_D405_RIGHT_SERIAL=260522274442
SITE_D435_FPS=30
SITE_D435_HEIGHT=720
SITE_D435_SERIAL=262522073161
SITE_D435_WIDTH=1280
SITE_EE_FRAME=Rizon4-063989_flange
SITE_EE_X_M=0.25
SITE_EE_Y_M=0.0
SITE_EE_Z_M=0.35
SITE_FASTDDS_PROFILE=
SITE_GO_HOME_POSE=home
SITE_GO_HOME_SPEED=0.25
SITE_GRIP_AUTO_INIT=true
SITE_GRIP_BACKEND=usb
SITE_GRIP_BAUD=115200
SITE_GRIP_DEVICE=GripperDahuanModbus
SITE_GRIP_FORCE=20
SITE_GRIP_FORCE_N=50.0
SITE_GRIP_MODEL=PGEA-100-20-W
SITE_GRIP_PORT=/dev/ttyUSB0
SITE_GRIP_RS485_RTS=
SITE_GRIP_SLAVE=1
SITE_GRIP_SN=26610799
SITE_GRIP_SPEED=50
SITE_GRIP_STROKE_MM=20.0
SITE_GRIP_VEL_MPS=0.05
SITE_GRIP_VENDOR=dh
SITE_HANDEYE_BOARD_COLS=8
SITE_HANDEYE_BOARD_ROWS=11
SITE_HANDEYE_CALIBRATE=/home/cc/ros2/handeye/calibrate.yaml
SITE_HANDEYE_FILE=/home/cc/ros2/handeye/results/89_eye_in_hand.yaml
SITE_HANDEYE_MODE=eye_in_hand
SITE_HANDEYE_MOVE_T=8.0
SITE_HANDEYE_PIN_HOLDS=2
SITE_HANDEYE_PIN_SEC=0.45
SITE_HANDEYE_SETTLE_QD=0.015
SITE_HANDEYE_SETTLE_SEC=1.5
SITE_HANDEYE_SETTLE_STABLE=0.6
SITE_HANDEYE_SETTLE_TO=8.0
SITE_HANDEYE_SKIP_UNSTABLE=true
SITE_HANDEYE_SQUARE_M=0.015
SITE_HANDEYE_TOUR=/home/cc/ros2/handeye/waypoints.yaml
SITE_HANDEYE_VEL_SCALE=0.05
SITE_HANDEYE_WAYPOINTS=/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml
SITE_HANDEYE_WORKDIR=/home/cc/ros2/handeye/tmp/89
SITE_HOLE_BACKEND=two_stage
SITE_HOLE_CONFIG=/home/cc/ros2/src/arm_application/config/hole_detect.yaml
SITE_HOLE_WEIGHTS=/home/cc/ros2/models/hole_insert/detect_best_1.onnx
SITE_HOLE_WORKPIECE_WEIGHTS=/home/cc/ros2/models/hole_insert/detect_best_1.onnx
SITE_JOINT_PREFIX=Rizon4-063989_
SITE_NAMED_POSES_FILE=/home/cc/ros2/src/arm_control/config/named_poses_89.yaml
SITE_NET_IFACE=enp4s0
SITE_PC_IP=192.168.2.10
SITE_ROBOT_ARM=left
SITE_ROBOT_CATALOG=Rizon4-063989,Rizon4-063991
SITE_ROBOT_IP=192.168.2.101
SITE_ROBOT_LABEL=左臂89
SITE_ROBOT_SN=Rizon4-063989
SITE_ROBOT_TOOL_NAME=
SITE_ROS2_ROOT=/home/cc/ros2
SITE_SERVO_WATCHDOG=1.2
SITE_SERVO_Z_REF=0.22
SITE_TACTILE_BACKEND=orisys
SITE_TACTILE_CONFIG=dd03
SITE_TACTILE_CONFIG_FILE=/home/cc/ros2/devices/sensors/orisys/models/dd03/config.json
SITE_TACTILE_LEFT=/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0
SITE_TACTILE_RIGHT=/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.2:1.0-video-index0
SITE_YOLO_CONF=0.1
SITE_YOLO_DEVICE=0
SITE_YOLO_IMGSZ=640
SITE_YOLO_MIN_CONF=0.12
SITE_YOLO_WEIGHTS=/home/cc/ros2/models/yolo/detect_best_0.onnx
SITE_YOLO_WEIGHTS_RESOLVED=/home/cc/ros2/models/yolo/detect_best_0.onnx
TERM=dumb
XML_CATALOG_FILES=file:///home/cc/anaconda3/envs/ros_flexiv/etc/xml/catalog file:///etc/xml/catalog
YOLO_AUTOINSTALL=false
_=/home/cc/anaconda3/envs/ros_gz_sim/bin/colcon
_CE_CONDA=
_CE_M=
_CONDA_EXE=/home/cc/anaconda3/bin/conda
_CONDA_ROOT=/home/cc/anaconda3
+1
View File
@@ -0,0 +1 @@
../../src/arm_kinematics/package.xml
@@ -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/arm_kinematics'
+1
View File
@@ -0,0 +1 @@
../../../src/arm_kinematics/resource/arm_kinematics
+1
View File
@@ -0,0 +1 @@
../../src/arm_kinematics/setup.cfg
+1
View File
@@ -0,0 +1 @@
../../src/arm_kinematics/setup.py
@@ -0,0 +1 @@
prepend-non-duplicate;PYTHONPATH;/home/cc/ros2/build/arm_kinematics
@@ -0,0 +1,3 @@
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX//home/cc/ros2/build/arm_kinematics"
@@ -0,0 +1,3 @@
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
_colcon_prepend_unique_value PYTHONPATH "/home/cc/ros2/build/arm_kinematics"