diff --git a/robot_client/robots/realman/motors_bus.py b/robot_client/robots/realman/motors_bus.py index af31ecfb..6482515e 100644 --- a/robot_client/robots/realman/motors_bus.py +++ b/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)