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

change pose control api
Some checks failed
Secret Leaks / trufflehog (push) Has been cancelled
Details

Browse Source
This commit is contained in:
yutang
2025-06-11 16:17:39 +08:00
parent f4f82c916f
commit f4fec8f51c
5 changed files with 48 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
lerobot/common/robot_devices/motors/realman.py
View File

@@ -90,9 +90,13 @@ class RealmanMotorsBus:
self.write(target_joint=self.init_joint_position)
def write(self, target_joint:list):
self.rmarm.rm_movej(target_joint[:-1], 50, 0, 0, 1)
# self.rmarm.rm_movej(target_joint[:-1], 50, 0, 0, 1)
self.rmarm.rm_movej_follow(target_joint[:-1])
self.rmarm.rm_set_gripper_position(target_joint[-1], block=False, timeout=2)
def write_endpose(self, target_endpose: list, gripper: int):
self.rmarm.rm_movej_p(target_endpose, 50, 0, 0, 1)
self.rmarm.rm_set_gripper_position(gripper, block=False, timeout=2)
def read(self) -> Dict:
"""

2
lerobot/common/robot_devices/robots/configs.py
View File

@@ -701,7 +701,7 @@ class RealmanRobotConfig(RobotConfig):
"joint_6": [6, "realman"],
"gripper": [7, "realman"],
},
init_joint = {'joint': [-90, 90, 90, 170, 90, -90, 1000]}
init_joint = {'joint': [-90, 90, 90, -90, -90, 90, 1000]}
)
}
)

62
lerobot/common/robot_devices/robots/realman.py
View File

@@ -28,24 +28,24 @@ class RealmanRobot:
# build realman motors
self.piper_motors = make_motors_buses_from_configs(self.config.left_follower_arm)
self.arm = self.piper_motors['main']
self.arm.rmarm.rm_movej(self.arm.init_joint_position[:-1], 50, 0, 0, 0)
self.arm.rmarm.rm_movej(self.arm.init_joint_position[:-1], 50, 0, 0, 1)
time.sleep(2)
ret = self.arm.rmarm.rm_get_current_arm_state()
init_pose = ret[1]['pose']
# build init teleop info
self.init_info = {
'init_joint': self.arm.init_joint_position,
'init_pose': init_pose,
'max_gripper': config.max_gripper,
'min_gripper': config.min_gripper,
'servo_config_file': config.servo_config_file
}
# build state-action cache
self.joint_queue = deque(maxlen=2)
# build gamepad teleop
if not self.inference_time:
# build init teleop info
self.init_info = {
'init_joint': self.arm.init_joint_position,
'init_pose': init_pose,
'max_gripper': config.max_gripper,
'min_gripper': config.min_gripper,
'servo_config_file': config.servo_config_file
}
self.teleop = HybridController(self.init_info)
else:
self.teleop = None
@@ -159,7 +159,6 @@ class RealmanRobot:
state = self.arm.read() # read current joint position from robot
action = self.teleop.get_action() # target joint position and pose end pos from gamepad
self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
# print('gripper', action['gripper'])
if action['control_mode'] == 'joint':
# 关节控制模式（主模式）
@@ -186,32 +185,25 @@ class RealmanRobot:
# do action
before_write_t = time.perf_counter()
# 使用笛卡尔空间运动控制末端位姿
if action['tozero']:
self.arm.rmarm.rm_movej(self.arm.init_joint_position[:-1], 50, 0, 0, 0)
ret = self.arm.rmarm.rm_get_current_arm_state()
target_joints = ret[1].get('joint', self.arm.init_joint_position)
current_pose = ret[1]['pose']
self.teleop.update_endpose_state(current_pose)
self.teleop.update_joint_state(target_joints)
self.teleop.update_tozero_state(False)
else:
result = self.arm.rmarm.rm_movej_p(target_pose, 50, 0, 0, 0)
# 夹爪控制
self.arm.rmarm.rm_set_gripper_position(action['gripper'], False, 2)
ret = self.arm.rmarm.rm_get_current_arm_state()
target_joints = ret[1].get('joint', self.arm.init_joint_position)
self.joint_queue.append(list(self.arm.read().values()))
self.teleop.update_joint_state(target_joints)
# result = self.arm.rmarm.rm_movej_p(target_pose, 100, 0, 0, 0)
self.arm.rmarm.rm_movep_follow(target_pose)
self.arm.rmarm.rm_set_gripper_position(action['gripper'], False, 2)
ret = self.arm.rmarm.rm_get_current_arm_state()
target_joints = ret[1].get('joint', self.arm.init_joint_position)
self.joint_queue.append(list(self.arm.read().values()))
self.teleop.update_joint_state(target_joints)
self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
print('-'*80)
print('mode: ', action['control_mode'])
print('state: ', list(state.values()))
print('action: ', target_joints)
print('cache[0]: ', self.joint_queue[0])
print('cache[-1]: ', self.joint_queue[-1])
print('-'*80)
# print('-'*80)
# print('mode: ', action['control_mode'])
# print('state: ', list(state.values()))
# print('action: ', target_joints)
# print('cache[0]: ', self.joint_queue[0])
# print('cache[-1]: ', self.joint_queue[-1])
# print('time: ', time.perf_counter() - before_write_t)
# print('-'*80)
# time.sleep(1)
if not record_data:
return

5
lerobot/common/robot_devices/teleop/gamepad.py
View File

@@ -399,11 +399,6 @@ class HybridController:
def update_tozero_state(self, tozero):
self.tozero = tozero
# def update_state(self, state):
# """更新状态信息（从机械臂获取当前状态）"""
# self.pose = state['end_pose']
# self.joint = state['joint']
# self.gripper = state['gripper']
def get_action(self) -> Dict:
"""获取当前控制命令"""

15
realman_src/movep_canfd.py Normal file
View File

@@ -0,0 +1,15 @@
from Robotic_Arm.rm_robot_interface import *
import time
# 实例化RoboticArm类
arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
# 创建机械臂连接，打印连接id
handle = arm.rm_create_robot_arm("192.168.3.18", 8080)
print(handle.id)
print(arm.rm_movep_follow([-0.330512, 0.255993, -0.161205, 3.141, 0.0, -1.57]))
time.sleep(2)
# print(arm.rm_movep_follow([0.3, 0, 0.3, 3.14, 0, 0]))
# time.sleep(2)
arm.rm_delete_robot_arm()