354 lines
12 KiB
Python
354 lines
12 KiB
Python
#!/usr/bin/env python3
|
||
# -*-coding:utf8-*-
|
||
from typing import Optional
|
||
import time
|
||
from Robotic_Arm.rm_robot_interface import *
|
||
import pygame
|
||
import threading
|
||
from typing import Dict
|
||
|
||
|
||
def enable_fun(arm: RoboticArm):
|
||
'''
|
||
使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
|
||
'''
|
||
enable_flag = False
|
||
# 设置超时时间（秒）
|
||
timeout = 5
|
||
# 记录进入循环前的时间
|
||
start_time = time.time()
|
||
elapsed_time_flag = False
|
||
|
||
while not enable_flag:
|
||
elapsed_time = time.time() - start_time
|
||
print("--------------------")
|
||
|
||
# 获取机械臂状态
|
||
ret = arm.rm_get_current_arm_state()
|
||
if ret[0] == 0: # 成功获取状态
|
||
arm_state = ret[1]
|
||
enable_flag = True
|
||
|
||
print("使能状态:", enable_flag)
|
||
print("--------------------")
|
||
# 检查是否超过超时时间
|
||
if elapsed_time > timeout:
|
||
print("超时....")
|
||
elapsed_time_flag = True
|
||
enable_flag = True
|
||
break
|
||
time.sleep(1)
|
||
|
||
if elapsed_time_flag:
|
||
print("程序自动使能超时,退出程序")
|
||
exit(0)
|
||
|
||
|
||
class EndPoseController:
|
||
def __init__(self, init_joint, init_pose):
|
||
# 初始化pygame和手柄
|
||
pygame.init()
|
||
pygame.joystick.init()
|
||
|
||
# 检查是否有连接的手柄
|
||
if pygame.joystick.get_count() == 0:
|
||
raise Exception("未检测到手柄")
|
||
|
||
# 初始化手柄
|
||
self.joystick = pygame.joystick.Joystick(0)
|
||
self.joystick.init()
|
||
|
||
# 摇杆死区
|
||
self.deadzone = 0.15
|
||
|
||
# 精细控制模式
|
||
self.fine_control_mode = False
|
||
|
||
# 初始化末端姿态 [X, Y, Z, RX, RY, RZ] XYZ meter RX RY RZ rad
|
||
self.init_joint = init_joint
|
||
self.init_pose = init_pose
|
||
self.joint = self.init_joint
|
||
self.pose = self.init_pose
|
||
self.pose_speeds = [0.0] * 6
|
||
|
||
# 末端位姿限制
|
||
self.pose_limits = [
|
||
(-0.850, 0.850), # X (m)
|
||
(-0.850, 0.850), # Y (m)
|
||
(0.850, 0.850), # Z (m) - 设置最小高度防止碰撞
|
||
(-3.14, 3.14), # RX (rad)
|
||
(-3.14, 3.14), # RY (rad)
|
||
(-3.14, 3.14) # RZ (rad)
|
||
]
|
||
|
||
# 控制参数
|
||
self.linear_step = 0.0015 # 线性移动步长(m)
|
||
self.angular_step = 0.001 # 角度步长(rad) - 从度转换为弧度
|
||
|
||
# 夹爪状态和速度
|
||
self.gripper_open = False
|
||
self.gripper_speed = 10
|
||
|
||
# 启动更新线程
|
||
self.running = True
|
||
self.thread = threading.Thread(target=self.update_controller)
|
||
self.thread.start()
|
||
|
||
print("机械臂末端位姿控制器已启动")
|
||
|
||
def _apply_nonlinear_mapping(self, value):
|
||
"""应用非线性映射以提高控制精度"""
|
||
# 保持符号，但对数值应用平方映射以提高精度
|
||
sign = 1 if value >= 0 else -1
|
||
return sign * (abs(value) ** 2)
|
||
|
||
def _normalize_angle(self, angle):
|
||
"""将角度归一化到[-π, π]范围内"""
|
||
import math
|
||
while angle > math.pi:
|
||
angle -= 2 * math.pi
|
||
while angle < -math.pi:
|
||
angle += 2 * math.pi
|
||
return angle
|
||
|
||
def update_controller(self):
|
||
while self.running:
|
||
try:
|
||
pygame.event.pump()
|
||
except Exception as e:
|
||
print(f"控制器错误: {e}")
|
||
self.stop()
|
||
continue
|
||
|
||
# 检查精细控制模式切换 (使用L3按钮)
|
||
if self.joystick.get_button(10): # L3按钮
|
||
self.fine_control_mode = not self.fine_control_mode
|
||
print(f"切换到{'精细' if self.fine_control_mode else '普通'}控制模式")
|
||
time.sleep(0.3) # 防止多次触发
|
||
|
||
# 检查重置按钮 (7号按钮，通常是Start按钮)
|
||
if self.joystick.get_button(7): # Start按钮
|
||
print("重置机械臂到初始位置...")
|
||
# 重置位姿
|
||
self.joint = self.init_joint
|
||
self.pose = self.init_pose
|
||
self.pose_speeds = [0.0] * 6
|
||
self.gripper_open = False
|
||
self.gripper_speed = 10
|
||
print("机械臂已重置到初始位置")
|
||
time.sleep(0.3) # 防止多次触发
|
||
|
||
# 更新末端位姿
|
||
self.update_end_pose()
|
||
|
||
# 夹爪控制（圈/叉）
|
||
circle = self.joystick.get_button(1) # 夹爪开
|
||
cross = self.joystick.get_button(0) # 夹爪关
|
||
self.gripper_speed = 10 if circle else (10 if cross else 0)
|
||
self.gripper_open = True if circle else False
|
||
|
||
# 更新夹爪
|
||
# self.gripper += self.gripper_speed
|
||
# self.gripper = max(0.0, min(0.1, self.gripper))
|
||
|
||
time.sleep(0.02)
|
||
|
||
def update_end_pose(self):
|
||
print("1", self.pose, "griper", self.gripper_open)
|
||
"""更新末端位姿控制"""
|
||
# 根据控制模式调整步长
|
||
current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0)
|
||
current_angular_step = self.angular_step * (0.1 if self.fine_control_mode else 1.0)
|
||
|
||
# print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
|
||
print(f"精细控制模式: {self.fine_control_mode}")
|
||
|
||
# 方向键控制XY
|
||
hat = self.joystick.get_hat(0)
|
||
hat_up = hat[1] == 1 # Y+
|
||
hat_down = hat[1] == -1 # Y-
|
||
hat_left = hat[0] == -1 # X-
|
||
hat_right = hat[0] == 1 # X+
|
||
|
||
# print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
|
||
|
||
# 右摇杆控制Z
|
||
right_y_raw = -self.joystick.get_axis(4)
|
||
# print(f"右摇杆原始值(axis 4): {self.joystick.get_axis(4)}")
|
||
# print(f"右摇杆处理值: {right_y_raw}")
|
||
|
||
# 左摇杆控制RZ
|
||
left_y_raw = -self.joystick.get_axis(1)
|
||
# print(f"左摇杆原始值(axis 1): {self.joystick.get_axis(1)}")
|
||
# print(f"左摇杆处理值: {left_y_raw}")
|
||
|
||
# 应用死区
|
||
right_y = 0.0 if abs(right_y_raw) < self.deadzone else right_y_raw
|
||
left_y = 0.0 if abs(left_y_raw) < self.deadzone else left_y_raw
|
||
|
||
# print(f"死区处理后 - 右摇杆: {right_y}, 左摇杆: {left_y}")
|
||
|
||
# 计算各轴速度
|
||
self.pose_speeds[1] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0) # Y
|
||
self.pose_speeds[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X
|
||
|
||
# 设置Z速度（右摇杆Y轴控制）
|
||
z_mapping = self._apply_nonlinear_mapping(right_y)
|
||
# print(f"Z轴非线性映射: {right_y} -> {z_mapping}")
|
||
self.pose_speeds[2] = z_mapping * current_linear_step # Z
|
||
|
||
# L1/R1控制RX旋转
|
||
LB = self.joystick.get_button(4) # RX-
|
||
RB = self.joystick.get_button(5) # RX+
|
||
self.pose_speeds[3] = (-current_angular_step if LB else (current_angular_step if RB else 0.0))
|
||
|
||
# △/□控制RY旋转
|
||
triangle = self.joystick.get_button(2) # RY+
|
||
square = self.joystick.get_button(3) # RY-
|
||
self.pose_speeds[4] = (current_angular_step if triangle else (-current_angular_step if square else 0.0))
|
||
|
||
# 左摇杆Y轴控制RZ旋转
|
||
rz_mapping = self._apply_nonlinear_mapping(left_y)
|
||
# print(f"RZ轴非线性映射: {left_y} -> {rz_mapping}")
|
||
self.pose_speeds[5] = rz_mapping * current_angular_step * 2 # RZ
|
||
|
||
# print(f"计算出的速度: {self.pose_speeds}")
|
||
|
||
# 更新末端位姿
|
||
old_pose = self.pose.copy()
|
||
for i in range(6):
|
||
self.pose[i] += self.pose_speeds[i]
|
||
|
||
# print(f"位姿更新: {old_pose} -> {self.pose}")
|
||
|
||
# 位置限制
|
||
# pose_before_limit = self.pose.copy()
|
||
# for i in range(3):
|
||
# min_val, max_val = self.pose_limits[i]
|
||
# self.pose[i] = max(min_val, min(max_val, self.pose[i]))
|
||
|
||
# if pose_before_limit != self.pose:
|
||
# print(f"位置限制生效: {pose_before_limit} -> {self.pose}")
|
||
|
||
# 角度归一化处理
|
||
pose_before_normalize = self.pose.copy()
|
||
for i in range(3, 6):
|
||
self.pose[i] = self._normalize_angle(self.pose[i])
|
||
|
||
# if pose_before_normalize != self.pose:
|
||
# print(f"角度归一化生效: {pose_before_normalize} -> {self.pose}")
|
||
|
||
# print("2", self.pose)
|
||
# print("=" * 50)
|
||
|
||
|
||
|
||
def update_state(self, end_pose, joint_state):
|
||
"""更新状态信息（从机械臂获取当前状态）"""
|
||
# 这里可以选择是否要同步机械臂的实际位置到控制器
|
||
# 如果需要严格同步，可以取消下面的注释
|
||
# self.pose = end_pose.copy()
|
||
pass
|
||
|
||
def get_action(self) -> Dict:
|
||
"""获取当前控制命令"""
|
||
return {
|
||
'X': self.pose[0],
|
||
'Y': self.pose[1],
|
||
'Z': self.pose[2],
|
||
'RX': self.pose[3],
|
||
'RY': self.pose[4],
|
||
'RZ': self.pose[5],
|
||
'gripper_speed': self.gripper_speed,
|
||
'gripper_open': self.gripper_open
|
||
}
|
||
|
||
def stop(self):
|
||
"""停止控制器"""
|
||
self.running = False
|
||
if self.thread.is_alive():
|
||
self.thread.join()
|
||
pygame.quit()
|
||
print("控制器已退出")
|
||
|
||
def reset(self):
|
||
"""重置到初始状态"""
|
||
self.joint = self.init_joint
|
||
self.pose = self.init_pose
|
||
self.pose_speeds = [0.0] * 6
|
||
self.gripper_open = False
|
||
self.gripper_speed = 10
|
||
print("已重置到初始状态")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
# 初始化睿尔曼机械臂
|
||
arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
|
||
# 创建机械臂连接
|
||
handle = arm.rm_create_robot_arm("192.168.3.18", 8080)
|
||
print(f"机械臂连接ID: {handle.id}")
|
||
|
||
# 使能机械臂
|
||
enable_fun(arm=arm)
|
||
|
||
# init joint
|
||
init_joint = [-90, 90, 90, -90, -90, 90]
|
||
arm.rm_movej(init_joint, 50, 0, 0, 1)
|
||
init_pose = arm.rm_get_current_arm_state()[1]['pose']
|
||
|
||
teleop = EndPoseController(init_joint, init_pose)
|
||
|
||
try:
|
||
while True:
|
||
# 获取当前控制命令
|
||
action = teleop.get_action()
|
||
|
||
# 构建目标位姿列表 [X, Y, Z, RX, RY, RZ]
|
||
target_pose = [
|
||
action['X'], # X (m)
|
||
action['Y'], # Y (m)
|
||
action['Z'], # Z (m)
|
||
action['RX'], # RX (rad)
|
||
action['RY'], # RY (rad)
|
||
action['RZ'] # RZ (rad)
|
||
]
|
||
|
||
# 使用笛卡尔空间直线运动控制末端位姿
|
||
# 参数: 目标位姿, 速度比例(20%), 交融半径(0), 连接标志(0), 阻塞模式(0-非阻塞)
|
||
result = arm.rm_movej_p(target_pose, 50, 0, 0, 0)
|
||
|
||
if result != 0:
|
||
print(f"运动控制错误，错误码: {result}")
|
||
|
||
if action['gripper_open']:
|
||
# arm.rm_set_gripper_release(action['gripper_speed'], block=True)
|
||
arm.rm_set_gripper_position(1000, True, 1)
|
||
else:
|
||
# arm.rm_set_gripper_pick(action['gripper_speed'], force=50, block=True)
|
||
arm.rm_set_gripper_position(1, True, 1)
|
||
|
||
# 获取当前机械臂状态
|
||
ret = arm.rm_get_current_arm_state()
|
||
if ret[0] == 0: # 成功获取状态
|
||
current_pose = ret[1].get('pose', target_pose)
|
||
current_joint = ret[1].get('joint', [0]*7)
|
||
|
||
teleop.update_state(current_pose, current_joint)
|
||
|
||
print("控制模式: 末端控制")
|
||
print(f"目标位姿: {target_pose}")
|
||
print(f"当前位姿: {current_pose}")
|
||
print(f"关节位置: {current_joint}")
|
||
else:
|
||
print(f"获取机械臂状态失败，错误码: {ret[0]}")
|
||
|
||
time.sleep(0.1)
|
||
|
||
except KeyboardInterrupt:
|
||
print("程序被用户中断")
|
||
finally:
|
||
# 清理资源
|
||
teleop.stop()
|
||
arm.rm_delete_robot_arm()
|
||
print("程序退出完成")
|