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/robot_arm.py b/src/Robot_arm_motion/robot_arm.py new file mode 100644 index 0000000000..ec35bf8ad0 --- /dev/null +++ b/src/Robot_arm_motion/robot_arm.py @@ -0,0 +1,250 @@ +import mujoco +import mujoco.viewer as viewer +import numpy as np +import time +import sys +import os + +# ========== 路径适配 ========== +SCENE_PATH = os.path.join(os.path.dirname(__file__), + "mujoco_menagerie-main", + "franka_emika_panda", + "grab_scene.xml") + +if not os.path.exists(SCENE_PATH): + print(f"❌ 场景文件不存在:{SCENE_PATH}") + sys.exit(1) + +# ========== 智能抓取控制器 ========== +class PandaAutoGrab: + def __init__(self): + self.model = mujoco.MjModel.from_xml_path(SCENE_PATH) + self.data = mujoco.MjData(self.model) + self.viewer = None + self.running = True + self.step_counter = 0 + self.current_phase = 0 + self.grab_complete = False + + # 机械臂参数 + self.ee_body_id = self.model.body("hand").id + self.joint_names = [f"joint{i}" for i in range(1, 8)] + self.joint_ids = [self.model.joint(name).id for name in self.joint_names] + self.gripper_joint_names = ["finger_joint1", "finger_joint2"] + + # 雅克比矩阵 + self.jacp = np.zeros((3, self.model.nv)) + self.jacr = np.zeros((3, self.model.nv)) + + # 抓取参数 + self.cube_body_id = self.model.body("cube").id + self.target_place_pos = np.array([0.3, 0.0, 0.1]) + self.gripper_open_pos = 0.04 + self.gripper_close_pos = 0.005 + self.safe_lift_height = 0.15 + self.grab_height = 0.05 + + # 打印模型信息 + print("="*50) + print("📌 模型Body列表:", [self.model.body(i).name for i in range(min(self.model.nbody, 10))]) + print("📌 模型Joint列表:", [self.model.joint(i).name for i in range(min(self.model.njnt, 10))]) + print("="*50) + + def get_ee_pos(self): + """获取末端执行器位置""" + return self.data.xpos[self.ee_body_id].copy() + + def get_cube_pos(self): + """获取立方体位置""" + return self.data.xpos[self.cube_body_id].copy() + + def _compute_jacobian(self): + """计算雅克比矩阵""" + mujoco.mj_jac(self.model, self.data, self.jacp, self.jacr, self.get_ee_pos(), self.ee_body_id) + return self.jacp[:, self.joint_ids] + + def _move_step(self, target, tol=0.003, speed=0.3): + """单步移动控制(修复维度匹配问题)""" + ee_pos = self.get_ee_pos() + error = target - ee_pos + error_norm = np.linalg.norm(error) + + if error_norm < tol: + return True # 到达目标 + + # 计算雅克比矩阵 + jacobian = self._compute_jacobian() # 3×7 + + # ========== 修正:正确的阻尼伪逆计算 ========== + # 方法1:使用正则化参数的伪逆(推荐) + lambda_ = 0.01 # 阻尼系数 + jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + lambda_ * np.eye(3)) + + # 方法2:若方法1仍报错,可改用numpy伪逆(自动处理维度) + # jacobian_pinv = np.linalg.pinv(jacobian, rcond=1e-3) + + # 关节速度指令 + joint_vel_cmd = speed * jacobian_pinv @ error + joint_vel_cmd = np.clip(joint_vel_cmd, -0.5, 0.5) # 速度限制 + + # PD力矩计算 + torque = np.zeros(7) + for i in range(7): + angle_error = joint_vel_cmd[i] * 0.1 + torque[i] = 250 * angle_error - 100 * self.data.qvel[self.joint_ids[i]] + torque[i] = np.clip(torque[i], -20, 20) + + # 设置关节力矩 + for i in range(7): + self.data.ctrl[self.joint_ids[i]] = torque[i] + + return False + + def _gripper_step(self, pos): + """单步夹爪控制""" + for j_name in self.gripper_joint_names: + j_id = self.model.joint(j_name).id + self.data.ctrl[j_id] = pos + return True + + def _grab_phase_machine(self): + """抓取状态机""" + if self.current_phase == 0: + # 阶段0:移动到初始位置 + if self._move_step(np.array([0.4, 0.0, 0.2])): + print("\n✅ 到达初始位置") + self.current_phase = 1 + self.step_counter = 0 + + elif self.current_phase == 1: + # 阶段1:获取立方体位置 + self.cube_pos = self.get_cube_pos() + print(f"\n🎯 识别到立方体位置:{np.round(self.cube_pos, 3)}") + self.current_phase = 2 + + elif self.current_phase == 2: + # 阶段2:移动到立方体上方 + if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4): + print("\n✅ 到达立方体上方") + self.current_phase = 3 + self.step_counter = 0 + + elif self.current_phase == 3: + # 阶段3:打开夹爪 + if self.step_counter == 0: + self._gripper_step(self.gripper_open_pos) + print("\n✋ 打开夹爪") + if self.step_counter > 100: # 等待夹爪动作 + self.current_phase = 4 + self.step_counter = 0 + self.step_counter += 1 + + elif self.current_phase == 4: + # 阶段4:下降抓取 + if self._move_step(self.cube_pos + np.array([0, 0, self.grab_height]), speed=0.2): + print("\n✅ 下降到抓取高度") + self.current_phase = 5 + self.step_counter = 0 + + elif self.current_phase == 5: + # 阶段5:闭合夹爪 + if self.step_counter == 0: + self._gripper_step(self.gripper_close_pos) + print("\n🤏 闭合夹爪抓取") + if self.step_counter > 100: + self.current_phase = 6 + self.step_counter = 0 + self.step_counter += 1 + + elif self.current_phase == 6: + # 阶段6:抬升立方体 + if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height + 0.05]), speed=0.3): + print("\n✅ 抬升立方体") + self.current_phase = 7 + self.step_counter = 0 + + elif self.current_phase == 7: + # 阶段7:移动到放置点上方 + if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4): + print("\n✅ 到达放置点上方") + self.current_phase = 8 + self.step_counter = 0 + + elif self.current_phase == 8: + # 阶段8:下降放置 + if self._move_step(self.target_place_pos + np.array([0, 0, self.grab_height]), speed=0.2): + print("\n✅ 下降到放置高度") + self.current_phase = 9 + self.step_counter = 0 + + elif self.current_phase == 9: + # 阶段9:释放立方体 + if self.step_counter == 0: + self._gripper_step(self.gripper_open_pos) + print("\n🫳 释放立方体") + if self.step_counter > 100: + self.current_phase = 10 + self.step_counter = 0 + self.step_counter += 1 + + elif self.current_phase == 10: + # 阶段10:撤离机械臂 + if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.3): + print("\n✅ 撤离机械臂") + self.current_phase = 11 + self.step_counter = 0 + + elif self.current_phase == 11: + # 阶段11:返回初始位置 + if self._move_step(np.array([0.4, 0.0, 0.2]), speed=0.4): + print("\n✅ 返回初始位置") + self.current_phase = 12 + + elif self.current_phase == 12: + # 阶段12:抓取完成 + if not self.grab_complete: + print("\n" + "="*50) + print("✅ 智能抓取任务完成!") + print("="*50) + self.grab_complete = True + + def run(self): + """单线程仿真主循环""" + # 初始化Viewer + self.viewer = viewer.launch_passive(self.model, self.data) + self.viewer.cam.azimuth = 70 + self.viewer.cam.elevation = -25 + self.viewer.cam.distance = 1.8 + self.viewer.cam.lookat = np.array([0.4, 0.0, 0.1]) + + print("\n🚀 仿真已启动,开始自动抓取...") + print("💡 关闭Viewer窗口可退出程序") + + # 单线程主循环 + while self.viewer.is_running(): + if self.running and not self.grab_complete: + self._grab_phase_machine() + else: + # 抓取完成后归零力矩 + for i in range(7): + self.data.ctrl[self.joint_ids[i]] = 0 + + mujoco.mj_step(self.model, self.data) + self.viewer.sync() + time.sleep(1/200) + + # 清理 + self.running = False + self.viewer.close() + print("\n👋 仿真结束") + +# ========== 主函数 ========== +if __name__ == "__main__": + try: + panda = PandaAutoGrab() + panda.run() + except Exception as e: + print(f"\n❌ 程序错误:{e}") + import traceback + traceback.print_exc() + sys.exit(1) \ No newline at end of file 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