import time from typing import Dict from piper_sdk import * from lerobot.common.robot_devices.motors.configs import PiperMotorsBusConfig class PiperMotorsBus: """ 对Piper SDK的二次封装 """ def __init__(self, config: PiperMotorsBusConfig): self.piper = C_PiperInterface_V2(config.can_name) self.piper.ConnectPort() self.motors = config.motors self.init_joint_position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [6 joints + 1 gripper] * 0.0 self.safe_disable_position = [0.0, 0.0, 0.0, 0.0, 0.52, 0.0, 0.0] self.pose_factor = 1000 # 单位 0.001mm self.joint_factor = 57324.840764 # 1000*180/3.14， rad -> 度（单位0.001度） @property def motor_names(self) -> list[str]: return list(self.motors.keys()) @property def motor_models(self) -> list[str]: return [model for _, model in self.motors.values()] @property def motor_indices(self) -> list[int]: return [idx for idx, _ in self.motors.values()] def connect(self, enable:bool) -> bool: ''' 使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序 ''' enable_flag = False loop_flag = False # 设置超时时间（秒） timeout = 5 # 记录进入循环前的时间 start_time = time.time() while not (loop_flag): elapsed_time = time.time() - start_time print(f"--------------------") enable_list = [] enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status) enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status) enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status) enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status) enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status) enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status) if(enable): enable_flag = all(enable_list) self.piper.EnableArm(7) self.piper.GripperCtrl(0,1000,0x01, 0) else: # move to safe disconnect position enable_flag = any(enable_list) self.piper.DisableArm(7) self.piper.GripperCtrl(0,1000,0x02, 0) print(f"使能状态: {enable_flag}") print(f"--------------------") if(enable_flag == enable): loop_flag = True enable_flag = True else: loop_flag = False enable_flag = False # 检查是否超过超时时间 if elapsed_time > timeout: print(f"超时....") enable_flag = False loop_flag = True break time.sleep(0.5) resp = enable_flag print(f"Returning response: {resp}") return resp def motor_names(self): return def set_calibration(self): return def revert_calibration(self): return def apply_calibration(self): """ 移动到初始位置 """ self.write("joints", target_joint=self.init_joint_position) def write(self, control_mode:str, target_joint:list): """ Joint control - target joint: in radians joint_1 (float): 关节1角度 (-92000~92000) / 57324.840764 joint_2 (float): 关节2角度 -1300 ~ 90000 / 57324.840764 joint_3 (float): 关节3角度 2400 ~ -80000 / 57324.840764 joint_4 (float): 关节4角度 -90000~90000 / 57324.840764 joint_5 (float): 关节5角度 19000~-77000 / 57324.840764 joint_6 (float): 关节6角度 -90000~90000 / 57324.840764 gripper_range: 夹爪角度 0~0.08 """ if control_mode == "end_pose": # 末端位姿控制 factor = 1000 X = round(target_joint[0]*factor*factor) Y = round(target_joint[1]*factor*factor) Z = round(target_joint[2]*factor*factor) RX = round(target_joint[3]*factor) RY = round(target_joint[4]*factor) RZ = round(target_joint[5]*factor) self.piper.MotionCtrl_2(0x01, 0x00, 100, 0x00) self.piper.EndPoseCtrl(X, Y, Z, RX, RY, RZ) else: # 关节控制 joint_0 = round(target_joint[0]*self.joint_factor) joint_1 = round(target_joint[1]*self.joint_factor) joint_2 = round(target_joint[2]*self.joint_factor) joint_3 = round(target_joint[3]*self.joint_factor) joint_4 = round(target_joint[4]*self.joint_factor) joint_5 = round(target_joint[5]*self.joint_factor) self.piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) # joint control # 0x00 标准模式 # self.piper.MotionCtrl_2(0x01, 0x01, 10, 0xAD) # joint control # mit模式 self.piper.JointCtrl(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5) # 夹爪控制 gripper_range = round(target_joint[6]*1000*1000) self.piper.GripperCtrl(abs(gripper_range), 1000, 0x01, 0) # 单位 0.001° # def write_endpose(self, target_pose: list): # X = round(target_pose[0] * 1000 * 1000) # m -> mm -> 0.001mm # Y = round(target_pose[1] * 1000 * 1000) # Z = round(target_pose[2] * 1000 * 1000) # RX = round(target_pose[3] * self.joint_factor) # rad -> 0.001deg # RY = round(target_pose[4] * self.joint_factor) # RZ = round(target_pose[5] * self.joint_factor) # gripper_range = round(target_pose[6]*1000*1000) # self.piper.EndPoseCtrl(X, Y, Z, RX, RY, RZ) # self.piper.GripperCtrl(abs(gripper_range), 1000, 0x01, 0) # 单位 0.001° def read(self) -> Dict: """ - 机械臂关节消息,单位0.001度 - 机械臂夹爪消息 """ joint_msg = self.piper.GetArmJointMsgs() joint_state = joint_msg.joint_state gripper_msg = self.piper.GetArmGripperMsgs() gripper_state = gripper_msg.gripper_state return { "joint_1": joint_state.joint_1 / self.joint_factor, "joint_2": joint_state.joint_2 / self.joint_factor, "joint_3": joint_state.joint_3 / self.joint_factor, "joint_4": joint_state.joint_4 / self.joint_factor, "joint_5": joint_state.joint_5 / self.joint_factor, "joint_6": joint_state.joint_6 / self.joint_factor, "gripper": gripper_state.grippers_angle / 1000 / 1000 } def safe_disconnect(self): """ Move to safe disconnect position """ self.write("joints", target_joint=self.safe_disable_position)