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

修复初始位姿初始化错误

Browse Source
This commit is contained in:
1002142102@qq.com
2025-12-10 17:21:54 +08:00
parent dce4e8ad8a
commit d038612734
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
robot_client/robots/realman/motors_bus.py
View File

@@ -86,6 +86,7 @@ class RealmanMotorsBus(MotorsBus):
if(enable_flag == enable): if(enable_flag == enable):
loop_flag = True loop_flag = True
enable_flag = True enable_flag = True
break
else: else:
loop_flag = False loop_flag = False
enable_flag = False enable_flag = False
@@ -96,8 +97,8 @@ class RealmanMotorsBus(MotorsBus):
enable_flag = True enable_flag = True
break break
time.sleep(1) time.sleep(1)
self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1) self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1)
ret = self.rmarm.rm_get_current_arm_state()
self.init_pose = ret[1]['pose'] self.init_pose = ret[1]['pose']
#读取夹状态 #读取夹状态
gripper_msg = self.rmarm.rm_get_gripper_state()[1] gripper_msg = self.rmarm.rm_get_gripper_state()[1]
@@ -178,7 +179,10 @@ class RealmanMotorsBus(MotorsBus):
values = list(actionData.values()) values = list(actionData.values())
for k,v in enumerate(self.init_pose): for k,v in enumerate(self.init_pose):
self.init_pose[k]+=values[k] self.init_pose[k]+=values[k]
r = self.rmarm.rm_movej_p(self.init_pose[:-1], 50, 0, 0, 0) r = self.rmarm.rm_movej_p(self.init_pose[:-1], 50, 0, 1, 0)
if r!=0:
print("movej error:", r)
raise Exception("movej error")
print(f"r:{r},pose:{self.init_pose}") print(f"r:{r},pose:{self.init_pose}")
# self.init_pose[6]+=actionData['gripper'] # self.init_pose[6]+=actionData['gripper']
# self.rmarm.rm_set_gripper_position(int(self.init_pose[6]*1000), False, 0) # self.rmarm.rm_set_gripper_position(int(self.init_pose[6]*1000), False, 0)