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

update state

Browse Source
This commit is contained in:
yutang
2025-06-09 16:23:09 +08:00
parent 55f284b306
commit d00c154db9
1 changed files with 25 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
realman_src/realman_mix_control.py
View File

@@ -95,9 +95,9 @@ class ServoArm:
# 返回默认配置
return {
"port": "/dev/ttyUSB0",
"baudrate": 9600,
"hex_data": "00 00 00 00",
"arm_axis": 7
"baudrate": 460800,
"hex_data": "55 AA 02 00 00 67",
"arm_axis": 6
}
def _bytes_to_signed_int(self, byte_data):
@@ -134,7 +134,6 @@ class ServoArm:
grasp_byte_data = bytearray.fromhex(grasp_hex)
# 夹爪进行归一化处理
grasp_value = self._bytes_to_signed_int(grasp_byte_data)/1000
# print(grasp_value)
joints["grasp"] = grasp_value
return joints
@@ -241,9 +240,8 @@ class HybridController:
]
# 控制参数
self.linear_step = 0.01 # 线性移动步长(m)
self.angular_step = 0.1 # 角度步长(rad)
self.joint_step = 0.5 # 关节角度步长(度)
self.linear_step = 0.005 # 线性移动步长(m)
self.angular_step = 0.05 # 角度步长(rad)
# 夹爪状态和速度
self.gripper_speed = 10
@@ -323,10 +321,8 @@ class HybridController:
# 从串口获取主臂关节状态
if self.servo_arm and self.servo_arm.connected:
# print("ok")
try:
self.master_joint_actions = self.servo_arm.get_joint_actions()
# print(self.master_joint_actions)
if self.master_joint_actions:
logging.debug(f"主臂关节状态: {self.master_joint_actions}")
@@ -342,10 +338,8 @@ class HybridController:
else:
# 末端控制模式，使用Xbox控制
self.update_end_pose()
time.sleep(0.02)
print('gripper:', self.gripper)
# print('gripper:', self.gripper)
def update_joint_control(self):
"""更新关节角度控制 - 优先使用主臂数据"""
@@ -434,10 +428,17 @@ class HybridController:
for i in range(3, 6):
self.pose[i] = self._normalize_angle(self.pose[i])
def update_state(self, end_pose, joint_state):
"""更新状态信息（从机械臂获取当前状态）"""
def update_joint_state(self, joint):
self.joint = joint
def update_endpose_state(self, end_pose):
self.pose = end_pose
self.joint = joint_state
def update_state(self, state):
"""更新状态信息（从机械臂获取当前状态）"""
self.pose = state['end_pose']
self.joint = state['joint']
self.gripper = state['gripper']
def get_action(self) -> Dict:
"""获取当前控制命令"""
@@ -453,7 +454,7 @@ class HybridController:
'RY': self.pose[4],
'RZ': self.pose[5],
},
'joint_angles': self.joint[:6], # 只取前6个关节
'joint_angles': self.joint,
'gripper': int(self.gripper)
}
@@ -519,6 +520,9 @@ if __name__ == "__main__":
if action['control_mode'] == 'joint':
# 关节控制模式（主模式）
target_joints = action['joint_angles']
ret = arm.rm_get_current_arm_state()
current_pose = ret[1].get('pose', init_pose)
teleop.update_endpose_state(current_pose)
else:
# 末端位姿控制模式
@@ -532,7 +536,10 @@ if __name__ == "__main__":
]
# 使用笛卡尔空间运动控制末端位姿
result = arm.rm_movej_p(target_pose, 50, 0, 0, 1)
result = arm.rm_movej_p(target_pose, 50, 0, 0, 0)
ret = arm.rm_get_current_arm_state()
current_joint = ret[1].get('joint', init_joint)
teleop.update_joint_state(current_joint)
# 夹爪控制
arm.rm_set_gripper_position(action['gripper'], True, 2)
@@ -541,9 +548,7 @@ if __name__ == "__main__":
ret = arm.rm_get_current_arm_state()
if ret[0] == 0: # 成功获取状态
current_pose = ret[1].get('pose', [0]*6)
current_joint = ret[1].get('joint', [0]*7)
# teleop.update_state(current_pose, current_joint)
current_joint = ret[1].get('joint', [0]*6)
print(f"当前位姿: {current_pose}")
print(f"当前关节: {current_joint[:6]}") # 只显示前6个关节