修复初始位姿初始化错误

robot_client/robots/realman/motors_bus.py

@@ -86,6 +86,7 @@ class RealmanMotorsBus(MotorsBus):
             if(enable_flag == enable):
                 loop_flag = True
                 enable_flag = True
+                break
             else: 
                 loop_flag = False
                 enable_flag = False
@@ -96,8 +97,8 @@ class RealmanMotorsBus(MotorsBus):
                 enable_flag = True
                 break
             time.sleep(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']
         #读取夹状态
         gripper_msg = self.rmarm.rm_get_gripper_state()[1]
@@ -178,7 +179,10 @@ class RealmanMotorsBus(MotorsBus):
             values = list(actionData.values())
             for k,v in enumerate(self.init_pose):
                 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}")
             # self.init_pose[6]+=actionData['gripper']
             # self.rmarm.rm_set_gripper_position(int(self.init_pose[6]*1000), False, 0)