diff --git a/realman_src/realman_mix_control.py b/realman_src/realman_mix_control.py index c19dd246..fdd6a8ba 100644 --- a/realman_src/realman_mix_control.py +++ b/realman_src/realman_mix_control.py @@ -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个关节