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

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

@@ -399,11 +399,6 @@ class HybridController:
def update_tozero_state(self, tozero): def update_tozero_state(self, tozero):
self.tozero = 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: 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()