import os import pybullet as p import pybullet_data import numpy as np import time import math from kuka import Kuka from debug_controller_6d import Debug6DController # ========== 状态机定义 ========== STATE_FIXED = 0 STATE_MOVING_TO_POS_1 = 1 STATE_MOVING_TO_POS_2 = 2 STATE_MOVING_TO_POS_3 = 3 STATE_MOVING_TO_POS_4 = 4 STATE_MOVING_TO_POS_5 = 5 STATE_MOVING_TO_POS_6 = 6 STATE_MOVING_TO_POS_7 = 7 # (STATE_MANUAL_JOG 已删除) # ========== 参数定义 ========== TIME_STEP = 1.0 / 240.0 # ========== 虚拟弹簧/阻抗参数 (手臂) ========== SPRING_K_POS = 10.0 # N/m SPRING_C_POS = 5.0 # N·s/m SPRING_K_ROT = 5.0 # Nm/rad SPRING_C_ROT = 1.0 # Nm·s/rad # 针对 3/4/5/6/7（物体附着）单独放软的弹簧参数 SPRING_K_POS_OBJ = 100.0 # N/m 更小刚度 SPRING_C_POS_OBJ = 15.0 # N·s/m 较大阻尼 SPRING_F_MAX_OBJ = 1 # N 限幅，避免冲击 # 弹簧连接模式：0=装在机械臂末端，1=装在被抓物体上 ATTACH_MODE_ARM = 0 ATTACH_MODE_OBJECT = 1 # ========== (V12: 夹爪弹簧参数已移除) ========== # ========== (V8) 3点弹簧 ========== # (V18) 这是机械臂末端的弹簧连接点 (相对于末端EE link) MULTI_ATTACH_POS_EE = np.array([ [ 0.02, 0.04, 0.21], [ 0.02, -0.00, 0.21], [ -0.02, 0.04, 0.23], [ -0.02, -0.00, 0.23] ]) print(f"使用 {len(MULTI_ATTACH_POS_EE)} 点弹簧系统。(已下移 20cm)") # (物体版) 弹簧连接点：基于 top.urdf 尺寸(0.40x0.10x0.02, 缩放0.5)，四角均匀分布 OBJ_HALF_X = 0.106 # ≈ 0.40/2*0.5 + 边段裕度 OBJ_HALF_Y = 0.022 # ≈ 0.10/2*0.5 OBJ_Z_TOP = 0.010 # 顶面高度 MULTI_ATTACH_POS_OBJECT = np.array([ [ OBJ_HALF_X, OBJ_HALF_Y, OBJ_Z_TOP], [ OBJ_HALF_X, -OBJ_HALF_Y, OBJ_Z_TOP], [-OBJ_HALF_X, OBJ_HALF_Y, OBJ_Z_TOP], [-OBJ_HALF_X, -OBJ_HALF_Y, OBJ_Z_TOP], ]) # --- (V18) 锚点位置 1 (数据) --- ANCHOR_1A_DATA = [0.585, 0.226, 0.065] ANCHOR_1B_DATA = [0.585, 0.190, 0.065] ANCHOR_1C_DATA = [0.621, 0.226, 0.065] ANCHOR_1D_DATA = [0.621, 0.190, 0.065] ANCHORS_DATA_1 = np.array([ ANCHOR_1A_DATA, ANCHOR_1B_DATA, ANCHOR_1C_DATA, ANCHOR_1D_DATA ]) # (V18) 为锚点 1 指定 Mode_ID = 0 (世界坐标) ANCHORS_DATA_1_MODE = 0 # --- (V18) 锚点位置 2 (数据) --- ANCHOR_2A_DATA = [0.585, 0.226, 0.055] ANCHOR_2B_DATA = [0.585, 0.190, 0.055] ANCHOR_2C_DATA = [0.621, 0.226, 0.055] ANCHOR_2D_DATA = [0.621, 0.190, 0.055] ANCHORS_DATA_2 = np.array([ ANCHOR_2A_DATA, ANCHOR_2B_DATA, ANCHOR_2C_DATA, ANCHOR_2D_DATA ]) # (V18) 为锚点 2 指定 Mode_ID = 1 (相对 'top_id' 的局部坐标) ANCHORS_DATA_2_MODE = 1 # --- (V18) 锚点位置 3 (数据) --- # 对称分布：以原数据中心 (约 0.626, -0.103, 0.175) 为中心，正方形 5cm 边长 ANCHOR3_CENTER = np.array([0.626, -0.103, 0.175]) ANCHOR3_OFF = 0.025 ANCHOR_3A_DATA = (ANCHOR3_CENTER + [ ANCHOR3_OFF, ANCHOR3_OFF, 0]).tolist() ANCHOR_3B_DATA = (ANCHOR3_CENTER + [ ANCHOR3_OFF, -ANCHOR3_OFF, 0]).tolist() ANCHOR_3C_DATA = (ANCHOR3_CENTER + [-ANCHOR3_OFF, ANCHOR3_OFF, 0]).tolist() ANCHOR_3D_DATA = (ANCHOR3_CENTER + [-ANCHOR3_OFF, -ANCHOR3_OFF, 0]).tolist() ANCHORS_DATA_3 = np.array([ ANCHOR_3A_DATA, ANCHOR_3B_DATA, ANCHOR_3C_DATA, ANCHOR_3D_DATA ]) # (V18) (假设) 为锚点 3 指定 Mode_ID = 1 (相对 'top_id' 的局部坐标) ANCHORS_DATA_3_MODE = 1 # --- (V18) 锚点位置 4 (数据) --- 单弹簧固定锚点（与 6 相同高度） ANCHORS_DATA_4 = np.array([[0.60, 0.20, 0.4]]) ANCHORS_DATA_4_MODE = 0 # 位置 5: 固定锚点（单点，与 4/6 同高） ANCHORS_DATA_5 = np.array([[0.60, -0.05, 0.4]]) # 位置 6: 固定世界锚点（两点） ANCHORS_DATA_6 = np.array([ [0.65, 0.00, 0.40], [0.65, -0.10, 0.40], ]) ANCHORS_DATA_6_MODE = 0 # 位置 7: top 上四点 (x,y 偏移 0.095, 0.024) 对应四个世界锚点（无旋转） ATTACH7_LOCAL = np.array([ [ 0.0475, -0.012, 0.0], # 右上 [ 0.0475, 0.012, 0.0], # 右下 [-0.0475, 0.012, 0.0], [-0.0475, -0.012, 0.0], ]) # 左上(x-,y+), 右上(x+,y+), 左下(x-,y-), 右下(x+,y-) 顺序对应四个锚点 ANCHORS_DATA_7 = np.array([ [0.6405, -0.055, 0.160], # ATTACH[0] 右下(x+, y-) [0.6135, -0.055, 0.180], # ATTACH[1] 右上(x+, y+) [0.6135, -0.145, 0.180], # ATTACH[2] 左上(x-, y+) [0.6405, -0.145, 0.160], # ATTACH[3] 左下(x-, y-) ]) ANCHORS_DATA_7_MODE = 0 TARGET_ORN_EULER = [math.pi, 0, -1.5500] TARGET_ORN_QUAT = p.getQuaternionFromEuler(TARGET_ORN_EULER) # ========== PyBullet 初始化 ========== p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(TIME_STEP) p.setGravity(0, 0, -9.8) # ========== (V13 JOG 控制滑块已删除) ========== # ================================================ # ========== 加载场景和机器人 ========== p.loadURDF("plane.urdf", basePosition=[0, 0, -0.05]) table_id = p.loadURDF("models/table_collision/table.urdf", [0.5, 0, -0.625], useFixedBase=True) # 复制一张桌子放在右侧 (桌宽1.5m + 0.2m 间距 => 1.7m 偏移) TABLE_X_SHIFT = 1.7 table2_id = p.loadURDF("models/table_collision/table.urdf", [0.5 + TABLE_X_SHIFT, 0, -0.625], useFixedBase=True) # 桌面 y+ 方向立牌标记 1/2 label_w, label_t, label_h = 0.12, 0.02, 0.3 z_table_top = -0.625 + 0.6 # 来自 table.urdf 中 0.6 的竖向偏移 label_pos_1 = [0.5, 0.6, z_table_top + label_h * 0.5] label_pos_2 = [0.5 + TABLE_X_SHIFT, 0.6, z_table_top + label_h * 0.5] label_shape_1 = p.createVisualShape(p.GEOM_BOX, halfExtents=[label_w*0.5, label_t*0.5, label_h*0.5], rgbaColor=[1, 0.3, 0.3, 1]) label_shape_2 = p.createVisualShape(p.GEOM_BOX, halfExtents=[label_w*0.5, label_t*0.5, label_h*0.5], rgbaColor=[0.3, 0.3, 1, 1]) p.createMultiBody(baseMass=0, baseVisualShapeIndex=label_shape_1, basePosition=label_pos_1) p.createMultiBody(baseMass=0, baseVisualShapeIndex=label_shape_2, basePosition=label_pos_2) p.addUserDebugText("1", [label_pos_1[0], label_pos_1[1], label_pos_1[2] + 0.2], textSize=2, textColorRGB=[1, 0.3, 0.3]) p.addUserDebugText("2", [label_pos_2[0], label_pos_2[1], label_pos_2[2] + 0.2], textSize=2, textColorRGB=[0.3, 0.3, 1]) # 调整初始视角，正对标牌（桌面 y+ 方向） p.resetDebugVisualizerCamera(cameraDistance=2.2, cameraYaw=0, cameraPitch=-20, cameraTargetPosition=[0.5 + TABLE_X_SHIFT * 0.5, 0.6, 0.3]) kuka = Kuka("models/kuka_iiwa/kuka_with_gripper2.sdf") print("Kuka 机器人已加载。") robot = kuka.kukaUid # ========== 新增：右侧第二套 Kuka (kukav1) ========== kukav1_uid = p.loadSDF("models/kuka_iiwa/kuka_with_gripper2.sdf")[0] p.resetBasePositionAndOrientation(kukav1_uid, [TABLE_X_SHIFT, 0, 0], [0, 0, 0, 1]) # 查找 ee link ee_link_index_kukav1 = None for i in range(p.getNumJoints(kukav1_uid)): link_name = p.getJointInfo(kukav1_uid, i)[12].decode('utf-8') if any(key in link_name for key in ["lbr_iiwa_link_7", "link_7", "tool0"]): ee_link_index_kukav1 = i break if ee_link_index_kukav1 is None: ee_link_index_kukav1 = p.getNumJoints(kukav1_uid) - 1 # 取前7个转动关节 arm_joint_indices_kukav1 = [] for j in range(p.getNumJoints(kukav1_uid)): if p.getJointInfo(kukav1_uid, j)[2] == p.JOINT_REVOLUTE and len(arm_joint_indices_kukav1) < 7: arm_joint_indices_kukav1.append(j) # 记录可动关节（用于 hold 位置控制） active_joint_indices_kukav1 = [j for j in range(p.getNumJoints(kukav1_uid)) if p.getJointInfo(kukav1_uid, j)[2] != p.JOINT_FIXED] # 初始保持当前位置，避免关节被释放 for j in active_joint_indices_kukav1: js = p.getJointState(kukav1_uid, j)[0] p.setJointMotorControl2(kukav1_uid, j, p.POSITION_CONTROL, targetPosition=js, force=200) # 基于位置的 6DOF 控制接口（使用当前关节角作为 IK 初值，减小抖动） def kukav1_set_pose(pos, orn_quat, force=200): joint_states = p.getJointStates(kukav1_uid, arm_joint_indices_kukav1) current_q = [s[0] for s in joint_states] ll = [-2.0] * len(arm_joint_indices_kukav1) ul = [ 2.0] * len(arm_joint_indices_kukav1) jr = [ 4.0] * len(arm_joint_indices_kukav1) q = p.calculateInverseKinematics( kukav1_uid, ee_link_index_kukav1, pos, orn_quat, lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=current_q ) for idx, j in enumerate(arm_joint_indices_kukav1): p.setJointMotorControl2( kukav1_uid, j, p.POSITION_CONTROL, targetPosition=q[idx], force=force, positionGain=0.1, velocityGain=1.0 ) # ========== 关节识别 ========== # 1. 识别末端 link (EE) ee_link_index = None for i in range(p.getNumJoints(robot)): link_name = p.getJointInfo(robot, i)[12].decode('utf-8') if any(key in link_name for key in ["lbr_iiwa_link_7", "link_7", "tool0"]): ee_link_index = i break if ee_link_index is None: ee_link_index = p.getNumJoints(robot) - 1 print(f"EE link index = {ee_link_index}") # 2. (硬编码) Kuka IIWA 手臂是 7-DOF arm_joint_indices = [0, 1, 2, 3, 4, 5, 6] print(f"Arm joint indices = {arm_joint_indices}") # 3. (硬编码) 夹爪的腕部旋转关节 gripper_to_arm_idx = 7 print(f"Gripper wrist joint index = {gripper_to_arm_idx}") # 4. (硬编码) 夹爪的两个手指关节 gripper_joint_indices = [8, 11] print(f"找到 {len(gripper_joint_indices)} 个夹爪主驱动关节: {gripper_joint_indices}") # === 5. (必需) active DOF：所有非固定关节 active_joint_indices = [j for j in range(p.getNumJoints(robot)) if p.getJointInfo(robot, j)[2] != p.JOINT_FIXED] arm_cols_in_active = [active_joint_indices.index(j) for j in arm_joint_indices] # 将 kukav1 初始关节姿态与主臂一致 for i in range(min(len(arm_joint_indices), len(arm_joint_indices_kukav1))): q = p.getJointState(robot, arm_joint_indices[i])[0] p.resetJointState(kukav1_uid, arm_joint_indices_kukav1[i], q) # === 6. 不再锁定手腕旋转关节 === # ======================================================= kuka.reset() initial_arm_q = [] for j in arm_joint_indices: initial_arm_q.append(p.getJointState(robot, j)[0]) print(f"初始固定角度: {np.round(initial_arm_q, 2)}") # 将 kukav1 关节初始化为与主臂相同的角度，并用位置控制保持 for i in range(min(len(initial_arm_q), len(arm_joint_indices_kukav1))): p.resetJointState(kukav1_uid, arm_joint_indices_kukav1[i], initial_arm_q[i]) p.setJointMotorControl2( kukav1_uid, arm_joint_indices_kukav1[i], p.POSITION_CONTROL, targetPosition=initial_arm_q[i], force=200, positionGain=0.1, velocityGain=1.0 ) # ========== 加载并缩放新物体 ========== print("在原点加载两个缩放后的物体...") stand_initial_pos = [0.6, 0.2, 0.1] stand_initial_orn = p.getQuaternionFromEuler([0, 0, math.pi/2]) body_initial_pos = [0.6, -0.1, 0.0] body_initial_orn = p.getQuaternionFromEuler([0, 0, 3*math.pi/2]) top_initial_pos = [0.6, 0.2, 0.25] top_initial_orn = p.getQuaternionFromEuler([math.pi/2, 0, math.pi/2]) stand_id = p.loadURDF("models/house/stand.urdf", basePosition=stand_initial_pos, baseOrientation=stand_initial_orn, globalScaling=0.5) body_id = p.loadURDF("models/house/main_body_fixed.urdf", basePosition=body_initial_pos, baseOrientation=body_initial_orn, globalScaling=0.5, useFixedBase=True) #body_id = p.loadURDF("/data/BCLearning/models/box_blue .urdf", basePosition=body_initial_pos, baseOrientation=body_initial_orn, globalScaling=1, useFixedBase=True) top_id = p.loadURDF("/models/house/top.urdf", basePosition=top_initial_pos, baseOrientation=top_initial_orn, globalScaling=0.5) # 依据木质密度约 500kg/m3，估算体积 9.7e-5 m3 => 约 0.05kg，增加阻尼防抖 p.changeDynamics(top_id, -1, mass=0.10, linearDamping=1.0, angularDamping=1.0) TOP_MASS = 0.10 # 复制一套房屋到右侧桌面（偏移 TABLE_X_SHIFT） stand_id_v2 = p.loadURDF("models/house/stand.urdf", basePosition=[stand_initial_pos[0] + TABLE_X_SHIFT, stand_initial_pos[1], stand_initial_pos[2]], baseOrientation=stand_initial_orn, globalScaling=0.5) body_id_v2 = p.loadURDF("models/house/main_body_fixed.urdf", basePosition=[body_initial_pos[0] + TABLE_X_SHIFT, body_initial_pos[1], body_initial_pos[2]], baseOrientation=body_initial_orn, globalScaling=0.5, useFixedBase=True) top_id_v2 = p.loadURDF("/models/house/top.urdf", basePosition=[top_initial_pos[0] + TABLE_X_SHIFT, top_initial_pos[1], top_initial_pos[2]], baseOrientation=top_initial_orn, globalScaling=0.5) p.changeDynamics(top_id_v2, -1, mass=0.10, linearDamping=1.0, angularDamping=1.0) # (===== V10 障碍物列表已删除 =====) # =================================== # ========== 初始化状态 ========== current_state = STATE_FIXED # (V18 MOD) 移除全局 anchor_frame_mode # (V18 MOD) 定义当前激活的锚点数据和其模式 active_anchors_data = ANCHORS_DATA_1 active_frame_mode = ANCHORS_DATA_1_MODE # 当前弹簧连接模式（默认装在机械臂末端） active_attach_mode = ATTACH_MODE_ARM # 记录 5 状态按下时的锚点（固定一次） anchor5_world = None # (V12) Kuka.py 风格的夹爪角度 (0.0=闭合, 0.4=张开) kuka_finger_angle = 0.0 dbg_line_ids = [-1] * len(MULTI_ATTACH_POS_EE) # (global_min_dist 已删除) # 辅助函数 def mat_from_quat(q): Rflat = p.getMatrixFromQuaternion(q) return np.array(Rflat, dtype=float).reshape(3, 3) # ... (您现有的代码) # (===== V10 障碍物列表已删除 =====) # =================================== # ========== (2) 新增：初始化 6D 调试控制器 ========== # (设置一个您认为合理的初始位置，例如在桌子旁边) initial_controller_pos = [0.5, -0.3, 0.2] initial_controller_orn = [0, 0, 0] controller_6d = Debug6DController( initial_pos=initial_controller_pos, initial_orn_euler=initial_controller_orn, axis_length=0.1, # (设置为 0.01 即为 1cm) pos_step=0.001 # 更慢移动 ) controller_6d.print_controls() # 打印键位说明 # =============================================== # ========== 主循环 ========== print("\n[模式] 状态机控制 (V19 - 增加Ctrl相对坐标)") # (V15 MOD) 修改键位说明 print(" [1] 激活弹簧 -> 位置 1 (Mode 0: World)") print(" [2] 激活弹簧 -> 位置 2 (Mode 1: Relative)") print(" [3] 激活弹簧 -> 位置 3 (Mode 0: World, attach OBJ)") print(" [4] 激活弹簧 -> 位置 4 (单弹簧 @固定锚点, attach OBJ)") print(" [5] 激活弹簧 -> 位置 5 (单弹簧 @固定锚点, attach OBJ)") print(" [6] 激活弹簧 -> 位置 6 (双弹簧 x±5cm, attach OBJ)") print(" [7] 激活弹簧 -> 位置 7 (top四角偏移, World anchors)") print(" [SPACE] 急停（清零速度/加速度），松开恢复") # ( 'Q' 键说明已删除 ) print(" [N] 夹爪 -> 张开一点") print(" [B] 夹爪 -> 闭合一点") # (V17 MOD) T 键说明已删除 print(" [R] 重置 (返回固定状态)") print(" [ESC] 退出.") ee_pos_world = np.array([0.0, 0.0, 0.0]) # (V19) Flag to check if top_pos was successfully read top_pos_valid = False # (防止初期打印引用未定义字符串) top_pos_str = "Top(World):(N/A, N/A, N/A)" # 控制器位置缓存，避免异常时未定义 controller_pos_world = np.array(initial_controller_pos, dtype=float) # top 锚点可视化 top_anchor_str = "Anchors:N/A" # 急停状态 emergency_stop_active = False freeze_arm_q = [] freeze_arm_q_v1 = [] resume_state = STATE_FIXED # 记录 5 状态按下时的锚点（固定一次） anchor5_world = None while True: keys_now = p.getKeyboardEvents() controller_6d.update(keys_now) if 27 in keys_now and keys_now[27] & p.KEY_WAS_TRIGGERED: break # SPACE 急停：按住时锁定当前姿态（不回原点），松开恢复 if p.B3G_SPACE in keys_now and (keys_now[p.B3G_SPACE] & p.KEY_IS_DOWN): if not emergency_stop_active: resume_state = current_state # 记住按下前的状态 freeze_arm_q = [p.getJointState(robot, j)[0] for j in arm_joint_indices] freeze_arm_q_v1 = [p.getJointState(kukav1_uid, j)[0] for j in arm_joint_indices_kukav1] emergency_stop_active = True # 清除弹簧可视化 if dbg_line_ids[0] != -1: for i in range(len(dbg_line_ids)): if dbg_line_ids[i] != -1: p.removeUserDebugItem(dbg_line_ids[i]) dbg_line_ids[i] = -1 print("\n[SPACE] 急停：锁定当前姿态") # 持续保持当前姿态 for i, j in enumerate(arm_joint_indices): p.setJointMotorControl2(robot, j, p.POSITION_CONTROL, targetPosition=freeze_arm_q[i], force=200, positionGain=0.1, velocityGain=1.0) for i, j in enumerate(arm_joint_indices_kukav1): p.setJointMotorControl2(kukav1_uid, j, p.POSITION_CONTROL, targetPosition=freeze_arm_q_v1[i], force=200, positionGain=0.1, velocityGain=1.0) p.stepSimulation() time.sleep(TIME_STEP) continue else: if emergency_stop_active: # 松开后恢复按下前的状态 current_state = resume_state emergency_stop_active = False # (V19 MOD) Get poses for relative calculation at the top of the loop try: # Get controller's world position (Debug6DController.position) controller_pos_world = np.array(controller_6d.position) # Get top_id's world pose top_pos, top_orn = p.getBasePositionAndOrientation(top_id) top_pos = np.array(top_pos) top_pos_str = f"TopXYZ:({top_pos[0]:.3f}, {top_pos[1]:.3f}, {top_pos[2]:.3f})" # Calculate relative position R_world_from_top = mat_from_quat(top_orn) R_top_from_world = R_world_from_top.T # Transpose is inverse for rotation matrix vec_world = controller_pos_world - top_pos vec_relative = R_top_from_world @ vec_world top_pos_valid = True # (V19) Flag success except Exception as e: # Handle cases where top_id or controller_6d.pos might not be ready/exist top_pos_valid = False # (V19) Define fallbacks so the spring logic doesn't crash top_pos = np.array([0,0,0]) R_world_from_top = np.identity(3) top_pos_str = "TopXYZ:(N/A, N/A, N/A)" top_anchor_str = "Anchors:N/A" # 保持 controller_pos_world 为上次成功值 # R: 重置 if ord('r') in keys_now and keys_now[ord('r')] & p.KEY_WAS_TRIGGERED: print("\n[R] 重置机器人和物体 (返回 STATE_FIXED)...") kuka.reset() kuka_finger_angle = 0.0 initial_arm_q = [] for j in arm_joint_indices: initial_arm_q.append(p.getJointState(robot, j)[0]) current_state = STATE_FIXED active_attach_mode = ATTACH_MODE_ARM active_anchors_data = ANCHORS_DATA_1 active_frame_mode = ANCHORS_DATA_1_MODE # 重置 kukav1 基座与关节 p.resetBasePositionAndOrientation(kukav1_uid, [TABLE_X_SHIFT, 0, 0], [0, 0, 0, 1]) for i in range(min(len(initial_arm_q), len(arm_joint_indices_kukav1))): p.resetJointState(kukav1_uid, arm_joint_indices_kukav1[i], initial_arm_q[i]) p.setJointMotorControl2( kukav1_uid, arm_joint_indices_kukav1[i], p.POSITION_CONTROL, targetPosition=initial_arm_q[i], force=200, positionGain=0.1, velocityGain=1.0 ) p.resetBasePositionAndOrientation(stand_id, stand_initial_pos, stand_initial_orn) p.resetBasePositionAndOrientation(body_id, body_initial_pos, body_initial_orn) p.resetBasePositionAndOrientation(top_id, top_initial_pos, top_initial_orn) p.resetBasePositionAndOrientation(stand_id_v2, [stand_initial_pos[0] + TABLE_X_SHIFT, stand_initial_pos[1], stand_initial_pos[2]], stand_initial_orn) p.resetBasePositionAndOrientation(body_id_v2, [body_initial_pos[0] + TABLE_X_SHIFT, body_initial_pos[1], body_initial_pos[2]], body_initial_orn) p.resetBasePositionAndOrientation(top_id_v2, [top_initial_pos[0] + TABLE_X_SHIFT, top_initial_pos[1], top_initial_pos[2]], top_initial_orn) # 状态切换 state_changed = False # (V15 MOD) 键位 ' ' -> '1' if ord('1') in keys_now and keys_now[ord('1')] & p.KEY_WAS_TRIGGERED: if current_state == STATE_FIXED: # (V15 MOD) print("\n[1] 激活! -> STATE_MOVING_TO_POS_1") for j in arm_joint_indices: p.setJointMotorControl2(robot, j, p.VELOCITY_CONTROL, force=0) state_changed = True if current_state != STATE_MOVING_TO_POS_1: print("\n切换到 -> POS_1 (Mode 0: World)") state_changed = True current_state = STATE_MOVING_TO_POS_1 # (V18 MOD) 设置数据和对应的模式 active_anchors_data = ANCHORS_DATA_1 active_frame_mode = ANCHORS_DATA_1_MODE active_attach_mode = ATTACH_MODE_ARM # (V15 MOD) 键位 'k' -> '2' if ord('2') in keys_now and keys_now[ord('2')] & p.KEY_WAS_TRIGGERED: if current_state == STATE_FIXED: # (V15 MOD) print("\n[2] 激活! -> STATE_MOVING_TO_POS_2") for j in arm_joint_indices: p.setJointMotorControl2(robot, j, p.VELOCITY_CONTROL, force=0) state_changed = True if current_state != STATE_MOVING_TO_POS_2: print("\n切换到 -> POS_2 (Mode 1: Relative)") state_changed = True current_state = STATE_MOVING_TO_POS_2 # (V18 MOD) 设置数据和对应的模式 active_anchors_data = ANCHORS_DATA_2 active_frame_mode = ANCHORS_DATA_2_MODE active_attach_mode = ATTACH_MODE_ARM # (V15 MOD) 键位 'm' -> '3' if ord('3') in keys_now and keys_now[ord('3')] & p.KEY_WAS_TRIGGERED: if current_state == STATE_FIXED: # (V15 MOD) print("\n[3] 激活! -> STATE_MOVING_TO_POS_3") for j in arm_joint_indices: p.setJointMotorControl2(robot, j, p.VELOCITY_CONTROL, force=0) state_changed = True if current_state != STATE_MOVING_TO_POS_3: print("\n切换到 -> POS_3 (Mode 0: World, attach OBJ)") state_changed = True current_state = STATE_MOVING_TO_POS_3 # (V18 MOD) 设置数据和对应的模式 active_anchors_data = ANCHORS_DATA_3 # 物体弹簧需要一个固定世界锚点，否则锚点跟着物体走会无力 active_frame_mode = ANCHORS_DATA_1_MODE # 强制用世界坐标 # 按键3开始使用“弹簧装在物体上”的模式 active_attach_mode = ATTACH_MODE_OBJECT # (V15 MOD) 键位 'l' -> '4' if ord('4') in keys_now and keys_now[ord('4')] & p.KEY_WAS_TRIGGERED: if current_state == STATE_FIXED: # (V15 MOD) print("\n[4] 激活! -> STATE_MOVING_TO_POS_4") for j in arm_joint_indices: p.setJointMotorControl2(robot, j, p.VELOCITY_CONTROL, force=0) state_changed = True if current_state != STATE_MOVING_TO_POS_4: print("\n切换到 -> POS_4 (Mode 0: World, attach OBJ)") state_changed = True current_state = STATE_MOVING_TO_POS_4 # 单弹簧固定锚点 active_anchors_data = ANCHORS_DATA_4 active_frame_mode = ANCHORS_DATA_4_MODE # 世界坐标 active_attach_mode = ATTACH_MODE_OBJECT # 键位 '5' if ord('5') in keys_now and keys_now[ord('5')] & p.KEY_WAS_TRIGGERED: if current_state == STATE_FIXED: print("\n[5] 激活! -> STATE_MOVING_TO_POS_5") for j in arm_joint_indices: p.setJointMotorControl2(robot, j, p.VELOCITY_CONTROL, force=0) state_changed = True if current_state != STATE_MOVING_TO_POS_5: print("\n切换到 -> POS_5 (top上方40cm, World, attach OBJ)") state_changed = True current_state = STATE_MOVING_TO_POS_5 # 单弹簧固定锚点 active_anchors_data = ANCHORS_DATA_5 active_frame_mode = 0 active_attach_mode = ATTACH_MODE_OBJECT # 键位 '6' if ord('6') in keys_now and keys_now[ord('6')] & p.KEY_WAS_TRIGGERED: if current_state == STATE_FIXED: print("\n[6] 激活! -> STATE_MOVING_TO_POS_6") for j in arm_joint_indices: p.setJointMotorControl2(robot, j, p.VELOCITY_CONTROL, force=0) state_changed = True if current_state != STATE_MOVING_TO_POS_6: print("\n切换到 -> POS_6 (单弹簧 @top原点, World anchor)") state_changed = True current_state = STATE_MOVING_TO_POS_6 active_anchors_data = ANCHORS_DATA_6 active_frame_mode = ANCHORS_DATA_6_MODE active_attach_mode = ATTACH_MODE_OBJECT # 键位 '7' if ord('7') in keys_now and keys_now[ord('7')] & p.KEY_WAS_TRIGGERED: if current_state == STATE_FIXED: print("\n[7] 激活! -> STATE_MOVING_TO_POS_7") for j in arm_joint_indices: p.setJointMotorControl2(robot, j, p.VELOCITY_CONTROL, force=0) state_changed = True if current_state != STATE_MOVING_TO_POS_7: print("\n切换到 -> POS_7 (top四点偏移, World anchors)") state_changed = True current_state = STATE_MOVING_TO_POS_7 active_anchors_data = ANCHORS_DATA_7 active_frame_mode = ANCHORS_DATA_7_MODE active_attach_mode = ATTACH_MODE_OBJECT # (V17 MOD) 移除了 'T' 键的切换逻辑 # (V13 'Q' 键 JOG 模式已删除) # (===== V12: 混合控制 - Kuka.py 风格的夹爪位置控制 =====) df = 0.0 if ord('n') in keys_now and keys_now[ord('n')] & p.KEY_WAS_TRIGGERED: # 'N' 键 张开 df = 0.5 print("\n[N] 夹爪 -> 张开一点") if ord('b') in keys_now and keys_now[ord('b')] & p.KEY_WAS_TRIGGERED: # 'B' 键 闭合 df = -0.5 print("\n[B] 夹爪 -> 闭合一点") # (1) 更新夹爪目标角度 kuka_finger_angle = np.clip(kuka_finger_angle + df, 0.0, 0.4) # (2) 应用夹爪位置控制 (现在 N/B 键在所有模式下都有效) p.setJointMotorControl2(robot, 8, p.POSITION_CONTROL, targetPosition=-kuka_finger_angle, force=2.5) p.setJointMotorControl2(robot, 11, p.POSITION_CONTROL, targetPosition=kuka_finger_angle, force=2.5) p.setJointMotorControl2(robot, 10, p.POSITION_CONTROL, targetPosition=0, force=2) p.setJointMotorControl2(robot, 13, p.POSITION_CONTROL, targetPosition=0, force=2) # ================================================================= # --- 3. 控制逻辑 (状态机) --- if current_state == STATE_FIXED: # (===== 状态: 固定 =====) p.setJointMotorControlArray( robot, arm_joint_indices, controlMode=p.POSITION_CONTROL, targetPositions=initial_arm_q, forces=[240.0] * len(arm_joint_indices), positionGains=[0.05] * len(arm_joint_indices), velocityGains=[1.0] * len(arm_joint_indices) ) if dbg_line_ids[0] != -1: for i in range(len(dbg_line_ids)): if dbg_line_ids[i] != -1: p.removeUserDebugItem(dbg_line_ids[i]) dbg_line_ids[i] = -1 ls_ee = p.getLinkState(robot, ee_link_index) ee_pos_world = np.array(ls_ee[0]) # (STATE_MANUAL_JOG 已删除) else: # (===== 状态: 弹簧控制 (POS_1, 2, 3, 4) =====) # 1) 收集状态 q_active = [] qd_active = [] for j in active_joint_indices: js = p.getJointState(robot, j) q_active.append(js[0]) qd_active.append(js[1]) q_active = np.array(q_active, dtype=float) qd_active = np.array(qd_active, dtype=float) zeros_jac = [0.0] * len(active_joint_indices) # 2) 重力补偿 qdd_active = [0.0] * len(active_joint_indices) tau_g_active = np.array(p.calculateInverseDynamics( robot, list(q_active), [0.0]*len(active_joint_indices), qdd_active )) tau_g_arm = tau_g_active[arm_cols_in_active] # 3) 获取末端状态 ls_ee = p.getLinkState(robot, ee_link_index, computeLinkVelocity=1, computeForwardKinematics=1) ee_pos_world = np.array(ls_ee[0]) ee_orn_world = np.array(ls_ee[1]) ee_vel_world = np.array(ls_ee[6]) ee_ang_vel_world = np.array(ls_ee[7]) R_world_from_ee = mat_from_quat(ee_orn_world) # --- 4a. (位置) tau_pos --- tau_pos_spring_arm = np.zeros(7) F_mag_total = 0.0 # 物体的姿态与速度（仅当弹簧装在物体时使用） top_lin_vel = np.zeros(3) top_ang_vel = np.zeros(3) if active_attach_mode == ATTACH_MODE_OBJECT: top_base_state = p.getBasePositionAndOrientation(top_id) top_vel_state = p.getBaseVelocity(top_id) top_pos_world = np.array(top_base_state[0]) top_orn_world = np.array(top_base_state[1]) top_lin_vel = np.array(top_vel_state[0]) top_ang_vel = np.array(top_vel_state[1]) R_world_from_top_obj = mat_from_quat(top_orn_world) # (V18 MOD) 根据当前激活的 active_frame_mode 计算有效的锚点世界坐标 effective_anchors = np.zeros_like(active_anchors_data) if active_frame_mode == 1: # 模式 1: active_anchors_data (数据) 是相对于 top_id 的 # (V19 MOD) Use pre-calculated values from the top of the loop if top_pos_valid: for i_anchor in range(len(active_anchors_data)): effective_anchors[i_anchor] = top_pos + R_world_from_top @ active_anchors_data[i_anchor] else: # Fallback if top_id failed (e.g., first frame) print(f"警告: 无法获取 top_id ({top_id}) 的位置, 强制使用世界坐标") effective_anchors = active_anchors_data else: # 模式 0: active_anchors_data (数据) 已经是世界坐标 effective_anchors = active_anchors_data # 根据状态决定使用的连接点列表（State4/5: 单点；State6: 两点 x±5cm；State7: 指定四点） if current_state in [STATE_MOVING_TO_POS_4, STATE_MOVING_TO_POS_5]: attach_pos_list = [np.array([0.0, 0.0, 0.0])] elif current_state == STATE_MOVING_TO_POS_6: attach_pos_list = [np.array([0.05, 0.0, 0.0]), np.array([-0.05, 0.0, 0.0])] elif current_state == STATE_MOVING_TO_POS_7: attach_pos_list = ATTACH7_LOCAL else: attach_pos_list = MULTI_ATTACH_POS_OBJECT if active_attach_mode == ATTACH_MODE_OBJECT else MULTI_ATTACH_POS_EE n_springs = len(attach_pos_list) for i in range(n_springs): # (V14) 根据4个状态设置不同颜色 if current_state == STATE_MOVING_TO_POS_1: color = [0, 1, 0] # Green elif current_state == STATE_MOVING_TO_POS_2: color = [0, 0, 1] # Blue elif current_state == STATE_MOVING_TO_POS_3: color = [1, 0, 1] # Magenta elif current_state == STATE_MOVING_TO_POS_4: color = [1, 1, 0] # Yellow elif current_state == STATE_MOVING_TO_POS_5: color = [0, 1, 1] # Cyan elif current_state == STATE_MOVING_TO_POS_6: color = [1, 0.5, 0] # Orange else: # POS_7 color = [0, 0.5, 1] # Blue-ish # (V15 MOD) 使用 effective_anchors anchor_pos_world = effective_anchors[i] if active_attach_mode == ATTACH_MODE_OBJECT: # 弹簧装在物体：用物体局部点作为连接点，对物体施加力 attach_pos_local = attach_pos_list[i] r_world_obj = R_world_from_top_obj @ attach_pos_local current_attach_pos_world = top_pos_world + r_world_obj current_attach_vel_world = top_lin_vel + np.cross(top_ang_vel, r_world_obj) disp = current_attach_pos_world - anchor_pos_world vel = current_attach_vel_world F_i = -SPRING_K_POS_OBJ * disp - SPRING_C_POS_OBJ * vel # 限幅避免数值爆炸 norm_F = np.linalg.norm(F_i) if norm_F > SPRING_F_MAX_OBJ: F_i = F_i * (SPRING_F_MAX_OBJ / norm_F) F_mag_total += float(np.linalg.norm(F_i)) # 将力直接作用在物体上 p.applyExternalForce( top_id, -1, forceObj=list(F_i), posObj=list(current_attach_pos_world), flags=p.WORLD_FRAME ) else: # 弹簧装在机械臂：保持原有雅可比求扭矩 attach_pos_local = attach_pos_list[i] r_world = R_world_from_ee @ attach_pos_local current_attach_pos_world = ee_pos_world + r_world current_attach_vel_world = ee_vel_world + np.cross(ee_ang_vel_world, r_world) disp = current_attach_pos_world - anchor_pos_world vel = current_attach_vel_world F_i = -SPRING_K_POS * disp - SPRING_C_POS * vel F_mag_total += float(np.linalg.norm(F_i)) Jt_i, Jr_i = p.calculateJacobian( robot, ee_link_index, list(attach_pos_local), list(q_active), list(zeros_jac), list(zeros_jac) ) Jv_i_arm = np.array(Jt_i)[:, arm_cols_in_active] tau_i = Jv_i_arm.T @ F_i tau_pos_spring_arm += tau_i dbg_line_ids[i] = p.addUserDebugLine( current_attach_pos_world, anchor_pos_world, color, 2.0, 0, replaceItemUniqueId=dbg_line_ids[i] ) # 移除多余的调试线（状态5只用1根） for i in range(n_springs, len(dbg_line_ids)): if dbg_line_ids[i] != -1: p.removeUserDebugItem(dbg_line_ids[i]) dbg_line_ids[i] = -1 # 额外负载重力补偿（顶板质量），包含力与力矩分配 tau_obj_gravity = np.zeros(7) if active_attach_mode == ATTACH_MODE_OBJECT and top_pos_valid: F_obj_up = np.array([0.0, 0.0, TOP_MASS * 9.8]) # 向上的补偿力 # 质心相对 EE 的向量，计算力矩补偿 r_world = top_pos_world - ee_pos_world torque_world = np.cross(r_world, F_obj_up) # 变换到局部，供雅可比计算（PyBullet 需要局部偏移） R_ee_from_world = R_world_from_ee.T attach_local_grav = R_ee_from_world @ r_world Jt_g, Jr_g = p.calculateJacobian( robot, ee_link_index, list(attach_local_grav), list(q_active), list(zeros_jac), list(zeros_jac) ) Jv_g_arm = np.array(Jt_g)[:, arm_cols_in_active] Jr_g_arm = np.array(Jr_g)[:, arm_cols_in_active] tau_force = Jv_g_arm.T @ F_obj_up tau_torque = Jr_g_arm.T @ torque_world tau_obj_gravity = tau_force + tau_torque # --- 5) 合成总扭矩 --- tau_cmd_arm = tau_g_arm + tau_obj_gravity + tau_pos_spring_arm # --- 6) 下发扭矩 --- p.setJointMotorControlArray( robot, arm_joint_indices, controlMode=p.TORQUE_CONTROL, forces=list(tau_cmd_arm) ) # === kukav1 跟随 A 的末端与夹爪（位置控制） === # 1) 读取 A 末端位姿 ee_state_a = p.getLinkState(robot, ee_link_index, computeForwardKinematics=1) ee_pos_a = ee_state_a[0] ee_orn_a = ee_state_a[1] # 1.1) 将 A 的位姿平移到 kukav1 工作区（沿 +X 平移 TABLE_X_SHIFT） ee_pos_b = np.array(ee_pos_a) + np.array([TABLE_X_SHIFT, 0.0, 0.0]) # 2) 用当前关节角作为 IK 初值，减小跳变 kukav1_set_pose(ee_pos_b, ee_orn_a, force=200) # 3) 同步夹爪开合角 grip_a = p.getJointState(robot, 8)[0] # A 夹爪一侧角度 p.setJointMotorControl2(kukav1_uid, 8, p.POSITION_CONTROL, targetPosition=grip_a, force=3.5) p.setJointMotorControl2(kukav1_uid, 11, p.POSITION_CONTROL, targetPosition=-grip_a, force=3.5) # --- (===== V10 碰撞查询已删除 =====) --- # ================================================= # --- 7. 状态输出 --- pose_str = f"Pose:({ee_pos_world[0]:.3f}, {ee_pos_world[1]:.3f}, {ee_pos_world[2]:.3f})" # (V12) Kuka.py 风格的夹爪状态 gripper_state_str = f"Angle:{kuka_finger_angle:.2f}" # (V19) ctrl_str (世界坐标) 和 ctrl_rel_str (相对坐标) 都在循环顶部计算好了 ctrl_str = controller_6d.get_pose_str() # (V18 MOD) 锚点坐标系状态 (现在是静态的) frame_mode_str = "REL" if active_frame_mode == 1 else "WORLD" attach_mode_str = "OBJ" if active_attach_mode == ATTACH_MODE_OBJECT else "ARM" # (dist_str 碰撞距离字符串已删除) if current_state == STATE_FIXED: state_str = "STATE: FIXED (LOCKED)" # (V19 MOD) 添加 ctrl_rel_str ctrl_xyz_str = f"CtrlXYZ:({controller_pos_world[0]:.3f}, {controller_pos_world[1]:.3f}, {controller_pos_world[2]:.3f})" print(f"\r[{state_str}] | {ctrl_xyz_str} | {top_pos_str} | {top_anchor_str}", end=" ") # (JOG 模式的状态输出已删除) else: # (V14) 弹簧模式的状态输出 (4个状态) if current_state == STATE_MOVING_TO_POS_1: state_str = "POS_1" color_str = "\033[92m" # Green elif current_state == STATE_MOVING_TO_POS_2: state_str = "POS_2" color_str = "\033[94m" # Blue elif current_state == STATE_MOVING_TO_POS_3: state_str = "POS_3" color_str = "\033[95m" # Magenta elif current_state == STATE_MOVING_TO_POS_4: state_str = "POS_4" color_str = "\033[93m" # Yellow else: # POS_5 state_str = "POS_5" color_str = "\033[96m" # Cyan reset_color = "\033[0m" # (移除了 | {dist_str}) # 仅输出控制标记的 XYZ ctrl_xyz_str = f"CtrlXYZ:({controller_pos_world[0]:.3f}, {controller_pos_world[1]:.3f}, {controller_pos_world[2]:.3f})" print(f"\r[{color_str}SPRING (4-pt){reset_color} -> {state_str}] | Attach:{attach_mode_str} | {ctrl_xyz_str} | {top_pos_str} | {top_anchor_str}", end="") # --- 8. 仿真步进 --- p.stepSimulation() time.sleep(TIME_STEP) print("\n仿真结束。") controller_6d.remove() p.disconnect()