diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 7769b8570c..7cb85e59f6 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -6,48 +6,69 @@ import time from contextlib import suppress -# ===================== 配置（已根据你的模型定制） ===================== +# ===================== 配置 ===================== warnings.filterwarnings('ignore') CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# --- 1. 任务清单（已使用正确的物体名称 'target_object'） --- +# --- 任务清单 --- TASK_QUEUE = [ - # 将名为 'target_object' 的物体移动到 (-0.3, 0, 0.05) ["target_object", [-0.3, 0, 0.05]], ] -# --- 2. 核心控制参数 --- -IK_GAIN = 1.5 -GRASP_FORCE = -8.0 # 夹爪闭合的力（负值表示向左/右） -CLEARANCE_HEIGHT = 0.25 # 移动时的安全高度 -STEP_PER_MOVE = 1200 # 移动到一个新位置所需的步数 -STEP_PER_GRASP = 400 # 抓取/释放动作所需的步数 +# --- 核心控制参数 (精细化) --- +IK_GAIN = 1.2 +CLEARANCE_HEIGHT = 0.25 +STEP_PER_MOVE = 1500 + +# --- 抓取力控制参数 (关键) --- +GRIPPER_OPEN_FORCE = 8.0 # 张开夹爪的力 +GRIPPER_GRASP_FORCE = -12.0 # 抓取时尝试闭合的力 +GRIPPER_HOLD_FORCE = -4.0 # 抓住物体后保持的力 +CONTACT_THRESHOLD = 1.0 # 接触力阈值 (N)，超过此值认为已接触 # ===================== 全局状态机 ===================== viewer = None current_task_index = 0 task_step = 0 +grasp_force_applied = 0.0 # 记录当前施加的抓取力 class TaskState: MOVE_TO_OBJECT_ABOVE = 1 - MOVE_DOWN_TO_GRASP = 2 - GRASP_OBJECT = 3 - MOVE_UP_AFTER_GRASP = 4 - MOVE_TO_TARGET_ABOVE = 5 - MOVE_DOWN_TO_PLACE = 6 - RELEASE_OBJECT = 7 - MOVE_UP_AFTER_RELEASE = 8 - FINISHED_ALL = 9 + PRE_GRASP_OPEN = 2 + MOVE_DOWN_TO_GRASP = 3 + GRASP_OBJECT = 4 + HOLD_OBJECT = 5 + MOVE_UP_AFTER_GRASP = 6 + MOVE_TO_TARGET_ABOVE = 7 + MOVE_DOWN_TO_PLACE = 8 + RELEASE_OBJECT = 9 + MOVE_UP_AFTER_RELEASE = 10 + FINISHED_ALL = 11 current_state = TaskState.MOVE_TO_OBJECT_ABOVE -# ===================== 核心功能函数 ===================== +# ===================== 辅助函数 ===================== +def get_contact_force(model, data, geom1_name, geom2_name): + """获取两个geom之间的接触力大小""" + geom1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom1_name) + geom2_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom2_name) + + for i in range(data.ncon): + con = data.contact[i] + if (con.geom1 == geom1_id and con.geom2 == geom2_id) or \ + (con.geom1 == geom2_id and con.geom2 == geom1_id): + # contact force is in data.cfrc_ext, but we need to compute it + # For simplicity, we can use the normal force magnitude + return abs(con.force[0]) + return 0.0 + + def simple_ik_control(model, data, ee_id, target_pos): - """逆运动学控制，让末端执行器移动到目标位置""" + """逆运动学控制""" current_pos = data.site_xpos[ee_id] error = target_pos - current_pos error = np.clip(error, -0.05, 0.05) @@ -57,18 +78,17 @@ def simple_ik_control(model, data, ee_id, target_pos): jnt_vel = np.dot(jacp[:, :3].T, error * IK_GAIN) jnt_vel = np.clip(jnt_vel, -0.5, 0.5) - # 注意：这里控制的是关节力矩（motor），而不是直接设置角度 - for i in range(min(3, model.nu - 2)): # 减去夹爪的两个控制 - data.ctrl[i] = jnt_vel[i] * 100 # 乘以一个系数来放大控制信号 + for i in range(min(3, model.nu)): + data.ctrl[i] = jnt_vel[i] * 100 -def run_smart_grasp_task(model, data, ee_id): - """智能抓取任务的状态机逻辑""" - global current_task_index, task_step, current_state +# ===================== 复杂抓取任务逻辑 ===================== +def run_complex_grasp_task(model, data, ee_id): + global current_task_index, task_step, current_state, grasp_force_applied if current_task_index >= len(TASK_QUEUE): if current_state != TaskState.FINISHED_ALL: - print("\n🎉🎉🎉 所有抓取任务已成功完成！🎉🎉🎉") + print("\n🎉🎉🎉 所有复杂抓取任务已成功完成！🎉🎉🎉") current_state = TaskState.FINISHED_ALL return False @@ -76,45 +96,64 @@ def run_smart_grasp_task(model, data, ee_id): obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, obj_name) if obj_id == -1: - print(f"❌ 错误：未在模型中找到物体 '{obj_name}'，请检查XML文件。") + print(f"❌ 错误：未在模型中找到物体 '{obj_name}'。") current_task_index += 1 return True # --- 状态机逻辑 --- if current_state == TaskState.MOVE_TO_OBJECT_ABOVE: - if task_step == 0: - print(f"\n[任务 {current_task_index + 1}/{len(TASK_QUEUE)}] 开始处理物体: {obj_name}") - print("-> 状态: 移动到物体上方...") + if task_step == 0: print("\n-> 状态: 移动到物体上方安全高度...") target_pos = data.xpos[obj_id].copy() target_pos[2] = CLEARANCE_HEIGHT simple_ik_control(model, data, ee_id, target_pos) if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.01: + task_step = 0 + current_state = TaskState.PRE_GRASP_OPEN + + elif current_state == TaskState.PRE_GRASP_OPEN: + if task_step == 0: print("-> 状态: 预抓取 - 张开夹爪...") + # 张开夹爪 + data.ctrl[3] = GRIPPER_OPEN_FORCE + data.ctrl[4] = -GRIPPER_OPEN_FORCE + if task_step > 500: # 等待夹爪完全张开 task_step = 0 current_state = TaskState.MOVE_DOWN_TO_GRASP elif current_state == TaskState.MOVE_DOWN_TO_GRASP: - if task_step == 0: - print("-> 状态: 下降以抓取物体...") + if task_step == 0: print("-> 状态: 下降接近物体...") target_pos = data.xpos[obj_id].copy() - target_pos[2] += 0.05 # 停在物体表面上方一点 + target_pos[2] += 0.04 # 非常接近物体表面 simple_ik_control(model, data, ee_id, target_pos) if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.005: task_step = 0 current_state = TaskState.GRASP_OBJECT elif current_state == TaskState.GRASP_OBJECT: - if task_step == 0: - print("-> 状态: 正在抓取...") - # 闭合夹爪: 左爪左移(负), 右爪右移(正) - data.ctrl[3] = GRASP_FORCE - data.ctrl[4] = -GRASP_FORCE - if task_step > STEP_PER_GRASP: + if task_step == 0: print("-> 状态: 抓取 - 快速闭合夹爪...") + # 快速闭合夹爪 + data.ctrl[3] = GRIPPER_GRASP_FORCE + data.ctrl[4] = -GRIPPER_GRASP_FORCE + # 检测是否接触到物体 + left_contact_force = get_contact_force(model, data, "gripper_left_geom", "target_geom") + right_contact_force = get_contact_force(model, data, "gripper_right_geom", "target_geom") + if left_contact_force > CONTACT_THRESHOLD or right_contact_force > CONTACT_THRESHOLD: + print(f" [接触检测] 检测到接触力！切换到保持模式。") + grasp_force_applied = GRIPPER_HOLD_FORCE + task_step = 0 + current_state = TaskState.HOLD_OBJECT + + elif current_state == TaskState.HOLD_OBJECT: + if task_step == 0: print("-> 状态: 保持 - 施加稳定夹持力...") + # 施加稳定的保持力 + data.ctrl[3] = grasp_force_applied + data.ctrl[4] = -grasp_force_applied + # 等待一小段时间确保抓稳 + if task_step > 500: task_step = 0 current_state = TaskState.MOVE_UP_AFTER_GRASP elif current_state == TaskState.MOVE_UP_AFTER_GRASP: - if task_step == 0: - print("-> 状态: 抓取成功，上升...") + if task_step == 0: print("-> 状态: 抓取成功，上升...") target_pos = data.site_xpos[ee_id].copy() target_pos[2] = CLEARANCE_HEIGHT simple_ik_control(model, data, ee_id, target_pos) @@ -122,9 +161,9 @@ def run_smart_grasp_task(model, data, ee_id): task_step = 0 current_state = TaskState.MOVE_TO_TARGET_ABOVE + # ... (MOVE_TO_TARGET_ABOVE, MOVE_DOWN_TO_PLACE 状态与之前类似) ... elif current_state == TaskState.MOVE_TO_TARGET_ABOVE: - if task_step == 0: - print(f"-> 状态: 移动到目标放置区上方 {target_place_pos[:2]}...") + if task_step == 0: print(f"-> 状态: 移动到目标放置区上方 {target_place_pos[:2]}...") target_pos = np.array(target_place_pos) target_pos[2] = CLEARANCE_HEIGHT simple_ik_control(model, data, ee_id, target_pos) @@ -133,8 +172,7 @@ def run_smart_grasp_task(model, data, ee_id): current_state = TaskState.MOVE_DOWN_TO_PLACE elif current_state == TaskState.MOVE_DOWN_TO_PLACE: - if task_step == 0: - print("-> 状态: 下降以放置物体...") + if task_step == 0: print("-> 状态: 下降到放置位置...") target_pos = np.array(target_place_pos) simple_ik_control(model, data, ee_id, target_pos) if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.005: @@ -142,18 +180,16 @@ def run_smart_grasp_task(model, data, ee_id): current_state = TaskState.RELEASE_OBJECT elif current_state == TaskState.RELEASE_OBJECT: - if task_step == 0: - print("-> 状态: 正在释放物体...") - # 打开夹爪: 左右爪都回中 - data.ctrl[3] = 0 - data.ctrl[4] = 0 - if task_step > STEP_PER_GRASP: + if task_step == 0: print("-> 状态: 释放 - 打开夹爪...") + # 完全松开夹爪 + data.ctrl[3] = GRIPPER_OPEN_FORCE + data.ctrl[4] = -GRIPPER_OPEN_FORCE + if task_step > 800: # 等待夹爪完全打开 task_step = 0 current_state = TaskState.MOVE_UP_AFTER_RELEASE elif current_state == TaskState.MOVE_UP_AFTER_RELEASE: - if task_step == 0: - print("-> 状态: 释放成功，上升并准备下一个任务...") + if task_step == 0: print("-> 状态: 释放成功，上升...") target_pos = data.site_xpos[ee_id].copy() target_pos[2] = CLEARANCE_HEIGHT simple_ik_control(model, data, ee_id, target_pos) @@ -166,7 +202,7 @@ def run_smart_grasp_task(model, data, ee_id): return True -# ===================== 主程序 ===================== +# ===================== 主程序 (与之前版本类似) ===================== def init(): global viewer if not os.path.exists(MODEL_PATH): @@ -186,9 +222,9 @@ def init(): raise ValueError("模型中必须包含一个名为 'ee_site' 的site。") print("=" * 60) - print("🚀 全自动智能抓取系统启动！") + print("🚀 全自动复杂柔顺抓取系统启动！") print(f"📋 任务清单: 共 {len(TASK_QUEUE)} 个物体需要处理。") - print("💡 正在连接到模型 'simple_arm'...") + print("💡 系统将使用接触感知进行智能抓取。") print("=" * 60) return model, data, ee_id @@ -199,7 +235,7 @@ def main(): model, data, ee_id = init() while viewer.is_alive: - if not run_smart_grasp_task(model, data, ee_id): + if not run_complex_grasp_task(model, data, ee_id): break mujoco.mj_step(model, data)