tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

手柄控制第一次提交

Browse Source
This commit is contained in:
yutang
2025-06-05 21:56:52 +08:00
parent a0ec9e1cb1
commit 83d6419d70
4 changed files with 201 additions and 246 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
realman.md
View File

@@ -23,7 +23,7 @@ conda install ffmpeg=7.1.1 -c conda-forge
Install Realman SDK:
```bash
pip install Robotic_Arm
pip install Robotic_Arm==1.0.4.1
pip install pygame
```

41
realman_src/dual_arm_connect_test.py
View File

@@ -1,18 +1,31 @@
from Robotic_Arm.rm_robot_interface import *
robot = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
handle = robot.rm_create_robot_arm("169.254.128.19", 8080)
print("机械臂ID：", handle.id)
armleft = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
armright = RoboticArm()
software_info = robot.rm_get_arm_software_info()
if software_info[0] == 0:
print("\n================== Arm Software Information ==================")
print("Arm Model: ", software_info[1]['product_version'])
print("Algorithm Library Version: ", software_info[1]['algorithm_info']['version'])
print("Control Layer Software Version: ", software_info[1]['ctrl_info']['version'])
print("Dynamics Version: ", software_info[1]['dynamic_info']['model_version'])
print("Planning Layer Software Version: ", software_info[1]['plan_info']['version'])
print("==============================================================\n")
else:
print("\nFailed to get arm software information, Error code: ", software_info[0], "\n")
lefthandle = armleft.rm_create_robot_arm("169.254.128.18", 8080)
print("机械臂ID：", lefthandle.id)
righthandle = armright.rm_create_robot_arm("169.254.128.19", 8080)
print("机械臂ID：", righthandle.id)
# software_info = armleft.rm_get_arm_software_info()
# if software_info[0] == 0:
# print("\n================== Arm Software Information ==================")
# print("Arm Model: ", software_info[1]['product_version'])
# print("Algorithm Library Version: ", software_info[1]['algorithm_info']['version'])
# print("Control Layer Software Version: ", software_info[1]['ctrl_info']['version'])
# print("Dynamics Version: ", software_info[1]['dynamic_info']['model_version'])
# print("Planning Layer Software Version: ", software_info[1]['plan_info']['version'])
# print("==============================================================\n")
# else:
# print("\nFailed to get arm software information, Error code: ", software_info[0], "\n")
print("Left: ", armleft.rm_get_current_arm_state())
print("Left: ", armleft.rm_get_arm_all_state())
armleft.rm_movej_p()
# print("Right: ", armright.rm_get_current_arm_state())
# 断开所有连接，销毁线程
RoboticArm.rm_destory()

387
realman_src/realman_demo.py
View File

@@ -2,13 +2,13 @@
# -*-coding:utf8-*-
from typing import Optional
import time
from piper_sdk import *
from Robotic_Arm.rm_robot_interface import *
import pygame
import threading
from typing import Dict
def enable_fun(piper:C_PiperInterface_V2):
def enable_fun(arm: RoboticArm):
'''
使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
'''
@@ -18,18 +18,18 @@ def enable_fun(piper:C_PiperInterface_V2):
# 记录进入循环前的时间
start_time = time.time()
elapsed_time_flag = False
while not (enable_flag):
while not enable_flag:
elapsed_time = time.time() - start_time
print("--------------------")
enable_flag = piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status and \
piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status and \
piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status and \
piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status and \
piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status and \
piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status
print("使能状态:",enable_flag)
piper.EnableArm(7)
piper.GripperCtrl(0,1000,0x01, 0)
# 获取机械臂状态
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:
@@ -38,13 +38,13 @@ def enable_fun(piper:C_PiperInterface_V2):
enable_flag = True
break
time.sleep(1)
pass
if(elapsed_time_flag):
if elapsed_time_flag:
print("程序自动使能超时,退出程序")
exit(0)
class UnifiedArmController:
class EndPoseController:
def __init__(self):
# 初始化pygame和手柄
pygame.init()
@@ -58,47 +58,32 @@ class UnifiedArmController:
self.joystick = pygame.joystick.Joystick(0)
self.joystick.init()
# 控制模式标志 - True为末端位姿控制，False为关节控制
self.end_pose_mode = False
# 摇杆死区
self.deadzone = 0.15
# 精细控制模式
self.fine_control_mode = False
# 初始化关节状态
self.joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 6个关节
self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 6个关节的速度
# 关节弧度限制
self.joint_limits = [
(-92000 / 57324.840764, 92000 / 57324.840764), # joint1
( 0 / 57324.840764, 120000 / 57324.840764), # joint2
(-80000 / 57324.840764, 0 / 57324.840764), # joint3
(-90000 / 57324.840764, 90000 / 57324.840764), # joint4
(-65000 / 57324.840764, 65000 / 57324.840764), # joint5
(-90000 / 57324.840764, 90000 / 57324.840764) # joint6
]
# 初始化末端姿态 [X, Y, Z, RX, RY, RZ]
self.pose = [0.056127, 0, 0.213266, 0, 84.999, 0]
# 初始化末端姿态 [X, Y, Z, RX, RY, RZ] XYZ meter RX RY RZ rad
self.init_joint = [10, -125, 100, 100, 0, 0, -10]
self.init_pose = [-0.185, 0.315, 0.080, -1.500, -0.800, -0.000]
self.joint = self.init_joint
self.pose = self.init_pose
self.pose_speeds = [0.0] * 6
# 末端位姿限制
self.pose_limits = [
(-0.6, 0.6), # X (m)
(-0.6, 0.6), # Y (m)
(0.05, 0.6), # Z (m) - 设置最小高度防止碰撞
(-180, 180), # RX (deg)
(-180, 180), # RY (deg)
(-180, 180) # RZ (deg)
(-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.05 # 角度步长(deg)
self.joint_step = 0.015 # 关节步长(rad)
self.angular_step = 0.001 # 角度步长(rad) - 从度转换为弧度
# 夹爪状态和速度
self.gripper = 0.0
@@ -109,9 +94,7 @@ class UnifiedArmController:
self.thread = threading.Thread(target=self.update_controller)
self.thread.start()
print("机械臂统一控制器已启动")
print("按下OPTIONS(Start)切换控制模式")
print("当前模式:", "末端位姿控制" if self.end_pose_mode else "关节控制")
print("机械臂末端位姿控制器已启动")
def _apply_nonlinear_mapping(self, value):
"""应用非线性映射以提高控制精度"""
@@ -120,11 +103,12 @@ class UnifiedArmController:
return sign * (abs(value) ** 2)
def _normalize_angle(self, angle):
"""将角度归一化到[-180, 180]范围内"""
while angle > 180:
angle -= 360
while angle < -180:
angle += 360
"""将角度归一化到[-π, π]范围内"""
import math
while angle > math.pi:
angle -= 2 * math.pi
while angle < -math.pi:
angle += 2 * math.pi
return angle
def update_controller(self):
@@ -136,12 +120,6 @@ class UnifiedArmController:
self.stop()
continue
# 检查控制模式切换 (使用左摇杆按钮)
if self.joystick.get_button(9): # 左摇杆按钮
self.end_pose_mode = not self.end_pose_mode
print(f"切换到{'末端位姿控制' if self.end_pose_mode else '关节控制'}模式")
time.sleep(0.3) # 防止多次触发
# 检查精细控制模式切换 (使用L3按钮)
if self.joystick.get_button(10): # L3按钮
self.fine_control_mode = not self.fine_control_mode
@@ -150,24 +128,20 @@ class UnifiedArmController:
# 检查重置按钮 (7号按钮，通常是Start按钮)
if self.joystick.get_button(7): # Start按钮
print("重置机械臂到0位...")
# 重置关节和位姿
self.joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print("重置机械臂到初始位置...")
# 重置位姿
self.joint = self.init_joint
self.pose = self.init_pose
self.pose_speeds = [0.0] * 6
self.gripper = 0.0
self.gripper_speed = 0.0
# 临时切换到关节控制模式
self.end_pose_mode = False
print("机械臂已重置到0位")
print("机械臂已重置到初始位置")
time.sleep(0.3) # 防止多次触发
# 根据控制模式获取输入并更新状态
if self.end_pose_mode:
self.update_end_pose()
else:
self.update_joints()
# 更新末端位姿
self.update_end_pose()
# 夹爪控制（圈/叉）- 两种模式下都保持一致
# 夹爪控制（圈/叉）
circle = self.joystick.get_button(1) # 夹爪开
cross = self.joystick.get_button(0) # 夹爪关
self.gripper_speed = 0.01 if circle else (-0.01 if cross else 0.0)
@@ -179,11 +153,15 @@ class UnifiedArmController:
time.sleep(0.02)
def update_end_pose(self):
print("1", self.pose)
"""更新末端位姿控制"""
# 根据控制模式调整步长
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+
@@ -191,21 +169,32 @@ class UnifiedArmController:
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 = -self.joystick.get_axis(4) # 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 = -self.joystick.get_axis(1) # 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) < self.deadzone else right_y
left_y = 0.0 if abs(left_y) < self.deadzone else left_y
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[0] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0) # X
self.pose_speeds[1] = current_linear_step if hat_left else (-current_linear_step if hat_right else 0.0) # Y
# 设置Z速度（右摇杆Y轴控制）
self.pose_speeds[2] = self._apply_nonlinear_mapping(right_y) * current_linear_step # Z
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-
@@ -218,117 +207,59 @@ class UnifiedArmController:
self.pose_speeds[4] = (current_angular_step if triangle else (-current_angular_step if square else 0.0))
# 左摇杆Y轴控制RZ旋转
self.pose_speeds[5] = self._apply_nonlinear_mapping(left_y) * current_angular_step * 2 # 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}")
# 位置限制
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]))
# 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])
def update_joints(self):
"""更新关节控制"""
# 根据控制模式调整步长
current_joint_step = self.joint_step * (0.1 if self.fine_control_mode else 1.0)
# 使用类似于末端位姿控制的映射，但直接控制关节
# 左摇杆控制关节1和2 (类似于末端位姿控制中的X和Y)
left_x = -self.joystick.get_axis(0) # 左摇杆x轴
left_y = -self.joystick.get_axis(1) # 左摇杆y轴
# 应用死区
left_x = 0.0 if abs(left_x) < self.deadzone else left_x
left_y = 0.0 if abs(left_y) < self.deadzone else left_y
# 右摇杆控制关节3和4
right_x = self.joystick.get_axis(3) # 右摇杆x轴
right_y = self.joystick.get_axis(4) # 右摇杆y轴
# 应用死区
right_x = 0.0 if abs(right_x) < self.deadzone else right_x
right_y = 0.0 if abs(right_y) < self.deadzone else right_y
# 方向键控制关节5和6
hat = self.joystick.get_hat(0)
up = hat[1] == 1
down = hat[1] == -1
left = hat[0] == -1
right = hat[0] == 1
# 映射输入到关节速度
self.joint_speeds[0] = left_x * current_joint_step # joint1速度
self.joint_speeds[1] = left_y * current_joint_step # joint2速度
self.joint_speeds[2] = right_y * current_joint_step # joint3速度
self.joint_speeds[3] = right_x * current_joint_step # joint4速度
self.joint_speeds[4] = -current_joint_step if up else (current_joint_step if down else 0.0) # joint5速度
self.joint_speeds[5] = current_joint_step if right else (-current_joint_step if left else 0.0) # joint6速度
# 积分速度到关节位置
for i in range(6):
self.joints[i] += self.joint_speeds[i]
# 关节范围保护
for i in range(6):
min_val, max_val = self.joint_limits[i]
self.joints[i] = max(min_val, min(max_val, self.joints[i]))
def update_state(self, ctrl_mode, end_pose, joint_state):
if ctrl_mode == 'end_pose':
_joint_state = [0] * 6
_joint_state[0] = joint_state.joint_1 / 57324.840764
_joint_state[1] = joint_state.joint_2 / 57324.840764
_joint_state[2] = joint_state.joint_3 / 57324.840764
_joint_state[3] = joint_state.joint_4 / 57324.840764
_joint_state[4] = joint_state.joint_5 / 57324.840764
_joint_state[5] = joint_state.joint_6 / 57324.840764
self.joints = _joint_state
else:
_end_pose = [0] * 6
_end_pose[0] = end_pose.X_axis / 1000 / 1000
_end_pose[1] = end_pose.Y_axis / 1000 / 1000
_end_pose[2] = end_pose.Z_axis / 1000 / 1000
_end_pose[3] = end_pose.RX_axis / 1000
_end_pose[4] = end_pose.RY_axis / 1000
_end_pose[5] = end_pose.RZ_axis / 1000
self.pose = _end_pose
# 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:
"""获取当前控制命令"""
if self.end_pose_mode:
# 返回末端位姿
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': self.gripper
}
else:
# 返回关节角度
return {
'joint0': self.joints[0],
'joint1': self.joints[1],
'joint2': self.joints[2],
'joint3': self.joints[3],
'joint4': self.joints[4],
'joint5': self.joints[5],
'gripper': self.gripper
}
def get_control_mode(self):
"""返回当前控制模式"""
return "end_pose" if self.end_pose_mode else "joints"
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': self.gripper
}
def stop(self):
"""停止控制器"""
@@ -340,9 +271,8 @@ class UnifiedArmController:
def reset(self):
"""重置到初始状态"""
self.joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.pose = [0.056127, 0, 0.213266, 0, 84.999, 0]
self.joint = self.init_joint
self.pose = self.init_pose
self.pose_speeds = [0.0] * 6
self.gripper = 0.0
self.gripper_speed = 0.0
@@ -350,66 +280,61 @@ class UnifiedArmController:
if __name__ == "__main__":
piper = C_PiperInterface_V2("can0")
piper.ConnectPort()
piper.EnableArm(7)
enable_fun(piper=piper)
piper.GripperCtrl(0,1000,0x01, 0)
# 初始化睿尔曼机械臂
arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
teleop = UnifiedArmController()
factor = 1000
# 创建机械臂连接
handle = arm.rm_create_robot_arm("169.254.128.18", 8080)
print(f"机械臂连接ID: {handle.id}")
while True:
# 获取当前控制命令
action = teleop.get_action()
control_mode = teleop.get_control_mode()
if control_mode == "end_pose":
# 末端位姿控制
position = list(action.values())
X = round(position[0]*factor*factor)
Y = round(position[1]*factor*factor)
Z = round(position[2]*factor*factor)
RX = round(position[3]*factor)
RY = round(position[4]*factor)
RZ = round(position[5]*factor)
joint_6 = round(position[6]*factor*factor)
# 使能机械臂
enable_fun(arm=arm)
teleop = EndPoseController()
try:
while True:
# 获取当前控制命令
action = teleop.get_action()
piper.MotionCtrl_2(0x01, 0x00, 100, 0x00)
piper.EndPoseCtrl(X, Y, Z, RX, RY, RZ)
piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0)
new_end_pose = piper.GetArmEndPoseMsgs().end_pose
new_joint_state = piper.GetArmJointMsgs().joint_state
teleop.update_state(control_mode, new_end_pose, new_joint_state)
print("控制模式: 末端控制")
print("末端位置", new_end_pose)
print("关节位置:", new_joint_state)
else:
# 关节控制
joints = list(action.values())
# 将关节角度转换为适合发送的格式
joint0 = round(joints[0] * 57324.840764) # 转换为机械臂期望的单位
joint1 = round(joints[1] * 57324.840764)
joint2 = round(joints[2] * 57324.840764)
joint3 = round(joints[3] * 57324.840764)
joint4 = round(joints[4] * 57324.840764)
joint5 = round(joints[5] * 57324.840764)
gripper = round(joints[6] * 1000 * 1000)
# 构建目标位姿列表 [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)
]
# 发送关节控制命令
piper.MotionCtrl_2(0x01, 0x01, 100, 0x00)
piper.JointCtrl(joint0, joint1, joint2, joint3, joint4, joint5)
piper.GripperCtrl(abs(gripper), 1000, 0x01, 0)
new_end_pose = piper.GetArmEndPoseMsgs().end_pose
new_joint_state = piper.GetArmJointMsgs().joint_state
teleop.update_state(control_mode, new_end_pose, new_joint_state)
# 使用笛卡尔空间直线运动控制末端位姿
# 参数: 目标位姿, 速度比例(20%), 交融半径(0), 连接标志(0), 阻塞模式(0-非阻塞)
result = arm.rm_movej_p(target_pose, 50, 0, 0, 1)
print("控制模式: 关节控制")
print("末端位置", new_end_pose)
print("关节位置:", new_joint_state)
if result != 0:
print(f"运动控制错误，错误码: {result}")
time.sleep(0.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("程序退出完成")

17
realman_src/single_arm_control_test.py Normal file
View File

@@ -0,0 +1,17 @@
from Robotic_Arm.rm_robot_interface import *
armleft = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
lefthandle = armleft.rm_create_robot_arm("169.254.128.18", 8080)
print("机械臂ID：", lefthandle.id)
print("Left: ", armleft.rm_get_current_arm_state())
print("Left: ", armleft.rm_get_arm_all_state())
armleft.rm_movej([10, -125, 100, 100, 0, 0, -10], 50, 0, 0, 1)
# armleft.rm_movej_p([-0.185, 0.315, 0.080, -1.500, -0.800, -0.000], 50, 0, 0, 1)
# armleft.rm_movel([-0.185, 0.315, 0.080, -1.500, -0.800, -0.000], 50, 0, 0, 1)
# 断开所有连接，销毁线程
RoboticArm.rm_destory()