diff --git a/src/Robot_arm_motion/requirements.txt b/src/Robot_arm_motion/requirements.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/Robot_arm_motion/scripts/robot_arm.py b/src/Robot_arm_motion/scripts/robot_arm.py new file mode 100644 index 0000000000..3a17f8b51a --- /dev/null +++ b/src/Robot_arm_motion/scripts/robot_arm.py @@ -0,0 +1,145 @@ +""" +MuJoCo机械臂仿真v0.3 +功能:基础2关节机械臂可视化+关节角度控制+夹爪开合控制+接触检测 +项目根目录:Robot_arm_motion +代码目录:scripts +作者:邓卓 +日期:2025-12-22 +版本:v0.3(实现夹爪与立方体接触检测) +""" +import mujoco +import mujoco.viewer as viewer +import numpy as np + +def create_simple_arm_model(): + """创建简易2关节机械臂+立方体模型的XML字符串""" + xml = """ + + + """ + return mujoco.MjModel.from_xml_string(xml) + +# 新增:接触检测函数 +def check_gripper_cube_contact(model, data, gripper_geom_names, cube_geom_name): + """ + 检测夹爪与立方体是否接触 + 参数: + model: MuJoCo模型 + data: MuJoCo数据 + gripper_geom_names: 夹爪几何名称列表 + cube_geom_name: 立方体几何名称 + 返回: + bool: True=接触,False=未接触 + """ + # 获取立方体几何的ID + cube_geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, cube_geom_name) + if cube_geom_id == -1: + print("错误:未找到立方体几何名称!") + return False + + # 遍历所有接触对 + for i in range(data.ncon): + contact = data.contact[i] + # 获取接触的两个几何ID + geom1_id = contact.geom1 + geom2_id = contact.geom2 + + # 检查是否是夹爪与立方体的接触 + for gripper_geom in gripper_geom_names: + gripper_geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, gripper_geom) + if gripper_geom_id == -1: + continue + if (geom1_id == gripper_geom_id and geom2_id == cube_geom_id) or \ + (geom1_id == cube_geom_id and geom2_id == gripper_geom_id): + return True + return False + +def main(): + """主函数:加载模型+启动仿真(新增接触检测)""" + # 加载模型 + model = create_simple_arm_model() + data = mujoco.MjData(model) + + # 设置初始关节角度 + joint1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "joint1") + joint2_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "joint2") + data.ctrl[joint1_id] = np.pi/4 # 关节1转45° + data.ctrl[joint2_id] = -np.pi/6 # 关节2转-30° + + # 设置夹爪初始开合角度(半开状态) + left_gripper_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "left_gripper_joint") + right_gripper_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "right_gripper_joint") + data.ctrl[left_gripper_id] = 0.03 # 左夹爪张开3mm + data.ctrl[right_gripper_id] = -0.03 # 右夹爪张开3mm + + # 定义夹爪和立方体的几何名称(用于接触检测) + gripper_geom_names = ["left_gripper_geom", "right_gripper_geom"] + cube_geom_name = "cube_geom" + + # 启动可视化 + print("=== Robot_arm_motion v0.3 仿真启动(新增接触检测) ===") + print("当前夹爪状态:半开(3mm)") + print("开始实时检测夹爪与立方体的接触状态...\n") + with viewer.launch_passive(model, data) as v: + last_contact_state = False # 记录上一帧的接触状态 + while v.is_running(): + # 检测夹爪与立方体的接触 + is_contact = check_gripper_cube_contact(model, data, gripper_geom_names, cube_geom_name) + + # 仅当状态变化时打印(避免控制台刷屏) + if is_contact != last_contact_state: + if is_contact: + print("✅ 检测到夹爪与立方体接触!") + else: + print("❌ 夹爪与立方体分离!") + last_contact_state = is_contact + + mujoco.mj_step(model, data) + v.sync() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/Robot_arm_motion/test.py b/src/Robot_arm_motion/test.py new file mode 100644 index 0000000000..e69de29bb2