hangX/lerobot
1
0
Fork 0
forked from tangger/lerobot
Code Pull Requests Activity

change pose control api

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

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