Compare commits
6 Commits
308f3938c3
...
aa1bc2bf75
| Author | SHA1 | Date | |
|---|---|---|---|
| aa1bc2bf75 | |||
| 3f4dfe02a7 | |||
| 197de7908a | |||
| aa7cd87151 | |||
| 33f1a31c59 | |||
| df436a9a31 |
@@ -23,6 +23,9 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
|
||||
**功能**：
|
||||
- 关节空间和笛卡尔空间运动控制
|
||||
- 完整的逆运动学和正运动学
|
||||
- **自动 z4 适配**：根据目标 z 坐标自动选择夹爪状态
|
||||
- `z > -55mm`: UP（J5=-100，z4=-100），工作范围 z ∈ [-190, 110]mm
|
||||
- `z ≤ -55mm`: DOWN（J5=81，z4=55），工作范围 z ∈ [-345, -55]mm
|
||||
- UDP 通信（与 ESP32）
|
||||
- 状态发布（10Hz）
|
||||
|
||||
@@ -31,9 +34,14 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
|
||||
|
||||
**功能**：
|
||||
- 相机坐标到基坐标系的自动转换
|
||||
- **自动处理相机旋转**：根据 J5 状态自动调整图像坐标转换
|
||||
- 抓取流程：释放 → 移动 → 抓取 → 回收
|
||||
- 释放流程：移动 → 释放 → 回收
|
||||
|
||||
**相机旋转说明**：
|
||||
- J5 < 0°（闭合/UP）：相机正向，`(xc, yc, zc) = (x_img, -y_img, z)`
|
||||
- J5 > 0°（张开/DOWN）：相机旋转 180°，`(xc, yc, zc) = (-x_img, y_img, z)`
|
||||
|
||||
## 🚀 快速开始
|
||||
|
||||
### 编译
|
||||
|
||||
@@ -4,12 +4,14 @@ float64 y # 目标 Y 坐标 (mm)
|
||||
float64 z # 目标 Z 坐标 (mm)
|
||||
float64 phi # 目标偏航角 (度)
|
||||
bool elbow_up # 是否使用肘部向上解
|
||||
uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合
|
||||
bool grip # 是否抓取
|
||||
bool release # 是否释放
|
||||
bool up # 夹爪朝上（z4=-100）
|
||||
bool down # 夹爪朝下（z4=55）
|
||||
bool grip # 是否抓取（J6=-5）
|
||||
bool release # 是否释放（J6=80）
|
||||
float64 duration # 运动时长 (秒，0 表示使用默认值)
|
||||
|
||||
# 夹爪状态常量
|
||||
# 已废弃：使用 up/down 代替
|
||||
uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合
|
||||
uint8 GRIPPER_KEEP = 0
|
||||
uint8 GRIPPER_OPEN = 1
|
||||
uint8 GRIPPER_CLOSED = 2
|
||||
|
||||
@@ -1,165 +0,0 @@
|
||||
# udp_teleop — ROS 2 底盘 + 机械臂键盘 UDP 遥控
|
||||
|
||||
通过键盘实时控制底盘（差速驱动）和机械臂（6 电机），指令通过 **单一 UDP socket** 发送到设备端。
|
||||
|
||||
## 项目结构
|
||||
|
||||
```
|
||||
ros2/
|
||||
├── build/ # colcon 构建产物（自动生成）
|
||||
├── install/ # colcon 安装产物（自动生成）
|
||||
├── log/ # 构建日志（自动生成）
|
||||
└── src/
|
||||
└── udp_teleop/ # ROS 2 包
|
||||
├── config/
|
||||
│ └── params.yaml # 可配置参数
|
||||
├── launch/ # launch 文件（预留）
|
||||
├── udp_teleop/
|
||||
│ ├── __init__.py
|
||||
│ └── keyboard_control.py # 键盘遥控节点
|
||||
├── resource/
|
||||
├── test/
|
||||
├── package.xml
|
||||
├── setup.cfg
|
||||
└── setup.py
|
||||
```
|
||||
|
||||
## 环境搭建
|
||||
|
||||
### 1. 安装 Conda
|
||||
|
||||
使用 Miniconda 或 Anaconda。推荐 Miniconda：
|
||||
|
||||
```bash
|
||||
# 下载并安装 Miniconda
|
||||
wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
|
||||
bash Miniconda3-latest-Linux-x86_64.sh
|
||||
```
|
||||
|
||||
### 2. 创建 ROS 2 Humble 环境
|
||||
|
||||
使用 robostack 频道安装 ROS 2 Humble Desktop 完整版：
|
||||
|
||||
```bash
|
||||
conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
|
||||
```
|
||||
|
||||
### 3. 安装构建工具
|
||||
|
||||
```bash
|
||||
conda activate ros2_humble
|
||||
conda install -c robostack-staging -c conda-forge \
|
||||
colcon-common-extensions \
|
||||
ros-humble-ament-cmake \
|
||||
python3-pip
|
||||
```
|
||||
|
||||
### 4. 安装 Python 依赖
|
||||
|
||||
```bash
|
||||
pip install pynput
|
||||
```
|
||||
|
||||
### 5. 激活环境
|
||||
|
||||
每次使用前：
|
||||
|
||||
```bash
|
||||
conda activate ros2_humble
|
||||
source /path/to/ros2/install/setup.bash # 首次构建后执行
|
||||
```
|
||||
|
||||
## 构建
|
||||
|
||||
```bash
|
||||
cd ros2
|
||||
colcon build --symlink-install --packages-select udp_teleop
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
> `--symlink-install`：修改 Python 源文件后无需重新构建，直接生效。
|
||||
|
||||
## 运行
|
||||
|
||||
### 使用参数文件（推荐）
|
||||
|
||||
```bash
|
||||
ros2 run udp_teleop keyboard_control \
|
||||
--ros-args --params-file src/udp_teleop/config/params.yaml
|
||||
```
|
||||
|
||||
### 命令行覆盖参数
|
||||
|
||||
```bash
|
||||
ros2 run udp_teleop keyboard_control \
|
||||
--ros-args -p udp_ip:=192.168.1.100 -p udp_port:=9999
|
||||
```
|
||||
|
||||
## 按键映射
|
||||
|
||||
### 底盘控制
|
||||
|
||||
| 按键 | 功能 |
|
||||
|------|------|
|
||||
| `W` / `S` | 前进 / 后退 |
|
||||
| `A` / `D` | 左移 / 右移 |
|
||||
| `Q` / `E` | 左转 / 右转 |
|
||||
|
||||
### 机械臂控制
|
||||
|
||||
| 按键 | 功能 |
|
||||
|------|------|
|
||||
| `↑` / `↓` | 升降高度（↑ 升高，↓ 降低） |
|
||||
| `2` ~ `6` | 选择关节 J2 ~ J6 |
|
||||
| `←` / `→` | 减小 / 增大当前关节角度 |
|
||||
|
||||
> 底盘和机械臂可以**同时操控**。机械臂指令仅在按下机械臂相关按键后发送。
|
||||
|
||||
### 其他
|
||||
|
||||
| 按键 | 功能 |
|
||||
|------|------|
|
||||
| `Ctrl+C` | 退出并发送底盘停止指令 |
|
||||
|
||||
## UDP 协议
|
||||
|
||||
### 底盘指令
|
||||
|
||||
```
|
||||
XYW:<X速度>:<Y速度>:<W角速度>:XZHY\n
|
||||
```
|
||||
|
||||
### 机械臂指令
|
||||
|
||||
```
|
||||
JXB:<高度>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
|
||||
```
|
||||
|
||||
- 6 个电机：电机 1 控制高度，电机 2~6 对应关节 J2~J6
|
||||
- 末尾补零至 8 个值
|
||||
|
||||
## 参数配置
|
||||
|
||||
| 参数 | 默认值 | 说明 |
|
||||
|------|--------|------|
|
||||
| `udp_ip` | `127.0.0.1` | UDP 目标 IP 地址 |
|
||||
| `udp_port` | `8888` | UDP 目标端口 |
|
||||
| `chassis_linear_speed` | `100` | 底盘线速度 |
|
||||
| `chassis_angular_speed` | `45` | 底盘角速度 |
|
||||
| `arm_height_step` | `5` | 高度每步变化量 |
|
||||
| `arm_joint_step` | `5` | 关节角度每步变化量 |
|
||||
| `update_rate` | `0.05` | 控制循环周期（秒） |
|
||||
| `stdin_hold_time` | `0.04` | 按键持续时间（秒），修复箭头键时序问题 |
|
||||
| `debug_keys` | `false` | 是否在状态行显示当前按键 |
|
||||
| `keyboard_backend` | `auto` | 键盘后端：`auto` / `stdin` / `pynput` / `win_poll` |
|
||||
|
||||
## 键盘后端
|
||||
|
||||
| 后端 | 说明 |
|
||||
|------|------|
|
||||
| `auto` | 自动选择：Linux/macOS 用 `stdin`，Windows 用 `win_poll` |
|
||||
| `stdin` | 基于终端原始输入，无需额外依赖，**需要交互终端** |
|
||||
| `pynput` | 基于 pynput 库，跨平台，需要 `pip install pynput` |
|
||||
| `win_poll` | Windows 专用，通过 Win32 API 轮询按键状态 |
|
||||
|
||||
> `ros2 launch` 启动的子进程**没有交互终端**，使用 `stdin` 后端会报错。必须通过 `ros2 run` 在终端直接运行。
|
||||
@@ -50,6 +50,12 @@ J5_OPEN = 81
|
||||
J5_CLOSED = -100
|
||||
DEFAULT_FIXED_J5 = J5_OPEN
|
||||
|
||||
# Z4 值根据夹爪状态变化
|
||||
# J5 = -100 (闭合) → 夹爪朝上 (UP) → z4 = -100
|
||||
# J5 = 81 (张开) → 夹爪朝下 (DOWN) → z4 = 55
|
||||
Z4_UP = -100 # 夹爪朝上（J5=-100，闭合）
|
||||
Z4_DOWN = 55 # 夹爪朝下（J5=81，张开）
|
||||
|
||||
GRIP_ANGLE = -5
|
||||
RELEASE_ANGLE = 80
|
||||
DEFAULT_FIXED_J6 = RELEASE_ANGLE
|
||||
@@ -61,7 +67,7 @@ DEFAULT_ZERO_J4 = 25
|
||||
DEFAULT_L1 = 125.0
|
||||
DEFAULT_L2 = 125.0
|
||||
DEFAULT_X4 = 110.0
|
||||
DEFAULT_Z4 = 80.0
|
||||
DEFAULT_Z4 = 80.0 # 仅用于配置默认值，实际使用动态 z4
|
||||
|
||||
DEFAULT_INTERP_DURATION = 1.0
|
||||
DEFAULT_INTERP_RATE = 20.0
|
||||
@@ -165,8 +171,32 @@ def normalize_angle_deg(angle_deg: float) -> float:
|
||||
return normalized
|
||||
|
||||
|
||||
def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
|
||||
"""正运动学：关节角度 → TCP 位姿"""
|
||||
def resolve_z4_from_j5(j5: int) -> float:
|
||||
"""根据 J5 状态确定 z4 值
|
||||
|
||||
- J5 = -100 (闭合): 夹爪朝上，z4 = -100mm
|
||||
- J5 = 81 (张开): 夹爪朝下，z4 = 55mm
|
||||
"""
|
||||
return Z4_UP if j5 < 0 else Z4_DOWN
|
||||
|
||||
|
||||
def resolve_j5_from_z(z: float) -> int:
|
||||
"""根据目标 z 坐标自动选择夹爪状态
|
||||
|
||||
- z > -55: 使用朝上状态 (J5=-100, z4=-100)
|
||||
- z <= -55: 使用朝下状态 (J5=81, z4=55)
|
||||
"""
|
||||
return J5_CLOSED if z > -55 else J5_OPEN
|
||||
|
||||
|
||||
def forward_kinematics(geometry: ArmGeometry, state: ArmMathState, z4: float) -> ArmPose:
|
||||
"""正运动学：关节角度 → TCP 位姿
|
||||
|
||||
Args:
|
||||
geometry: 机械臂几何参数
|
||||
state: 数学坐标系的关节状态
|
||||
z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定）
|
||||
"""
|
||||
theta2 = math.radians(state.theta2_deg)
|
||||
theta3 = math.radians(state.theta3_deg)
|
||||
theta4 = math.radians(state.theta4_deg)
|
||||
@@ -183,7 +213,7 @@ def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
|
||||
|
||||
x = j4_center_x + geometry.x4 * math.cos(phi)
|
||||
y = j4_center_y + geometry.x4 * math.sin(phi)
|
||||
z = state.d1 - geometry.z4
|
||||
z = state.d1 - z4 # 使用动态 z4
|
||||
|
||||
return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi))
|
||||
|
||||
@@ -195,13 +225,24 @@ def inverse_kinematics(
|
||||
elbow_up: bool,
|
||||
j5: int,
|
||||
j6: int,
|
||||
z4: float,
|
||||
) -> ArmMathState:
|
||||
"""逆运动学：TCP 位姿 → 关节角度"""
|
||||
"""逆运动学：TCP 位姿 → 关节角度
|
||||
|
||||
Args:
|
||||
geometry: 机械臂几何参数
|
||||
pose: 目标 TCP 位姿
|
||||
limits: 关节限位
|
||||
elbow_up: 肘部朝上/朝下
|
||||
j5: J5 角度
|
||||
j6: J6 角度
|
||||
z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定）
|
||||
"""
|
||||
# 计算 J4 中心位置
|
||||
phi = math.radians(pose.phi_deg)
|
||||
j4_x = pose.x - geometry.x4 * math.cos(phi)
|
||||
j4_y = pose.y - geometry.x4 * math.sin(phi)
|
||||
j4_z = pose.z + geometry.z4
|
||||
j4_z = pose.z + z4 # 使用动态 z4
|
||||
|
||||
d1 = j4_z
|
||||
|
||||
@@ -565,14 +606,17 @@ class ArmControlNode(Node):
|
||||
phi_deg=request.phi
|
||||
)
|
||||
|
||||
# 解析夹爪状态
|
||||
if request.gripper_state == SetGripper.Request.GRIPPER_OPEN:
|
||||
j5 = J5_OPEN
|
||||
elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED:
|
||||
j5 = J5_CLOSED
|
||||
# 解析 z4：优先级 up/down > 自动选择
|
||||
if request.up:
|
||||
z4 = Z4_UP # 夹爪朝上，z4=-100
|
||||
elif request.down:
|
||||
z4 = Z4_DOWN # 夹爪朝下，z4=55
|
||||
else:
|
||||
j5 = self.current_state.j5
|
||||
# 自动选择：根据目标 z 坐标
|
||||
j5_auto = resolve_j5_from_z(target_pose.z)
|
||||
z4 = resolve_z4_from_j5(j5_auto)
|
||||
|
||||
# 解析夹爪开合（J6：grip/release）
|
||||
if request.grip:
|
||||
j6 = GRIP_ANGLE
|
||||
elif request.release:
|
||||
@@ -580,6 +624,9 @@ class ArmControlNode(Node):
|
||||
else:
|
||||
j6 = self.current_state.j6
|
||||
|
||||
# 根据 z4 反推 J5（用于 UDP 命令）
|
||||
j5 = J5_CLOSED if z4 == Z4_UP else J5_OPEN
|
||||
|
||||
# 逆运动学
|
||||
math_state = inverse_kinematics(
|
||||
geometry=self.geometry,
|
||||
@@ -588,6 +635,7 @@ class ArmControlNode(Node):
|
||||
elbow_up=request.elbow_up,
|
||||
j5=j5,
|
||||
j6=j6,
|
||||
z4=z4,
|
||||
)
|
||||
|
||||
# 转换为命令状态
|
||||
@@ -628,19 +676,22 @@ class ArmControlNode(Node):
|
||||
return response
|
||||
|
||||
math_state = command_to_math_state(self.current_state, self.zero_offsets)
|
||||
pose = forward_kinematics(self.geometry, math_state)
|
||||
|
||||
# 根据当前 J5 状态确定 z4
|
||||
z4 = resolve_z4_from_j5(self.current_state.j5)
|
||||
pose = forward_kinematics(self.geometry, math_state, z4)
|
||||
|
||||
response.success = True
|
||||
response.x = pose.x
|
||||
response.y = pose.y
|
||||
response.z = pose.z
|
||||
response.phi = pose.phi_deg
|
||||
response.height = self.current_state.height
|
||||
response.j2 = self.current_state.j2
|
||||
response.j3 = self.current_state.j3
|
||||
response.j4 = self.current_state.j4
|
||||
response.j5 = self.current_state.j5
|
||||
response.j6 = self.current_state.j6
|
||||
response.x = float(pose.x)
|
||||
response.y = float(pose.y)
|
||||
response.z = float(pose.z)
|
||||
response.phi = float(pose.phi_deg)
|
||||
response.height = int(self.current_state.height)
|
||||
response.j2 = int(self.current_state.j2)
|
||||
response.j3 = int(self.current_state.j3)
|
||||
response.j4 = int(self.current_state.j4)
|
||||
response.j5 = int(self.current_state.j5)
|
||||
response.j6 = int(self.current_state.j6)
|
||||
|
||||
except Exception as e:
|
||||
response.success = False
|
||||
@@ -707,14 +758,15 @@ class ArmControlNode(Node):
|
||||
|
||||
# 计算并发布 TCP 位姿
|
||||
math_state = command_to_math_state(self.current_state, self.zero_offsets)
|
||||
pose = forward_kinematics(self.geometry, math_state)
|
||||
z4 = resolve_z4_from_j5(self.current_state.j5)
|
||||
pose = forward_kinematics(self.geometry, math_state, z4)
|
||||
|
||||
pose_msg = TCPPose()
|
||||
pose_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
pose_msg.x = pose.x
|
||||
pose_msg.y = pose.y
|
||||
pose_msg.z = pose.z
|
||||
pose_msg.phi = pose.phi_deg
|
||||
pose_msg.x = float(pose.x)
|
||||
pose_msg.y = float(pose.y)
|
||||
pose_msg.z = float(pose.z)
|
||||
pose_msg.phi = float(pose.phi_deg)
|
||||
self.pub_tcp_pose.publish(pose_msg)
|
||||
|
||||
except Exception as e:
|
||||
|
||||
@@ -71,10 +71,14 @@ def tcp_to_base(
|
||||
"""TCP 坐标系 → 机械臂基坐标系（水平相机版本）"""
|
||||
phi = math.radians(tcp_phi_deg)
|
||||
|
||||
# 旋转矩阵：TCP → 基坐标系
|
||||
# TCP X → 基坐标 -Y
|
||||
# TCP Y → 基坐标 -Z
|
||||
# TCP Z → 基坐标 X
|
||||
R_tcp_to_base = np.array([
|
||||
[-math.sin(phi), 0, math.cos(phi)],
|
||||
[ math.cos(phi), 0, math.sin(phi)],
|
||||
[0, -1, 0]
|
||||
[-math.sin(phi), 0, math.cos(phi)], # X_base
|
||||
[-math.cos(phi), 0, -math.sin(phi)], # Y_base (修正：添加负号)
|
||||
[0, -1, 0] # Z_base
|
||||
])
|
||||
|
||||
P_tcp = np.array([xt, yt, zt])
|
||||
@@ -183,8 +187,8 @@ class VisionGraspNode(Node):
|
||||
self.get_logger().info(' - /vision_grasp/grasp_target (geometry_msgs/Point)')
|
||||
self.get_logger().info(' - /vision_grasp/release_target (geometry_msgs/Point)')
|
||||
|
||||
def get_current_tcp_pose(self) -> Tuple[float, float, float, float]:
|
||||
"""查询当前 TCP 位姿"""
|
||||
def get_current_tcp_pose(self) -> Tuple[float, float, float, float, int]:
|
||||
"""查询当前 TCP 位姿（包括 J5 状态）"""
|
||||
req = GetPose.Request()
|
||||
future = self.get_pose_cli.call_async(req)
|
||||
|
||||
@@ -202,7 +206,7 @@ class VisionGraspNode(Node):
|
||||
if not result.success:
|
||||
raise RuntimeError(f"获取 TCP 位姿失败: {result.message}")
|
||||
|
||||
return result.x, result.y, result.z, result.phi
|
||||
return result.x, result.y, result.z, result.phi, result.j5
|
||||
|
||||
def move_to(self, x: float, y: float, z: float, phi: float,
|
||||
duration: float, grip: bool = False, release: bool = False) -> bool:
|
||||
@@ -280,18 +284,31 @@ class VisionGraspNode(Node):
|
||||
def _execute_grasp_thread(self, x: float, y: float, z: float):
|
||||
"""在独立线程中执行抓取流程"""
|
||||
try:
|
||||
# 转换坐标：(x, y, z) -> (xc, yc, zc) = (x, -y, z)
|
||||
xc = x
|
||||
yc = -y
|
||||
zc = z
|
||||
# 获取当前 TCP 位姿（包括 J5 状态）
|
||||
tcp_x, tcp_y, tcp_z, tcp_phi, j5 = self.get_current_tcp_pose()
|
||||
self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°, j5={j5}°')
|
||||
|
||||
self.get_logger().info(f'转换后相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
|
||||
# 图像坐标到相机坐标系转换
|
||||
# 相机水平安装，但会随 J5 状态旋转 180°
|
||||
# J5 = -100 (闭合) → 夹爪朝上 (UP) → 相机正向
|
||||
# J5 = 81 (张开) → 夹爪朝下 (DOWN) → 相机旋转 180°
|
||||
|
||||
# 获取当前 TCP 位姿
|
||||
tcp_x, tcp_y, tcp_z, tcp_phi = self.get_current_tcp_pose()
|
||||
self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°')
|
||||
if j5 < 0:
|
||||
# J5 闭合（UP）：相机正向
|
||||
# 图像 Y 向下 → 相机 -Y
|
||||
xc = x
|
||||
yc = -y
|
||||
zc = z
|
||||
self.get_logger().info(f'J5={j5}° (UP)，相机正向，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
|
||||
else:
|
||||
# J5 张开（DOWN）：相机旋转 180°
|
||||
# 图像 X,Y 都翻转（相对于 UP 状态）
|
||||
xc = -x
|
||||
yc = y
|
||||
zc = z
|
||||
self.get_logger().info(f'J5={j5}° (DOWN)，相机旋转 180°，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
|
||||
|
||||
# 坐标变换
|
||||
# 坐标变换：相机 → 基坐标系
|
||||
target_x, target_y, target_z = camera_to_base(
|
||||
xc, yc, zc,
|
||||
tcp_x, tcp_y, tcp_z, tcp_phi,
|
||||
@@ -323,7 +340,7 @@ class VisionGraspNode(Node):
|
||||
"""在独立线程中执行释放流程"""
|
||||
try:
|
||||
# 获取当前 TCP 位姿（用于获取 phi）
|
||||
_, _, _, tcp_phi = self.get_current_tcp_pose()
|
||||
_, _, _, tcp_phi, _ = self.get_current_tcp_pose()
|
||||
|
||||
# 执行释放流程
|
||||
self.execute_release(x, y, z, tcp_phi)
|
||||
|
||||
Reference in New Issue
Block a user