Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Empty file.
250 changes: 250 additions & 0 deletions src/Robot_arm_motion/robot_arm.py
Original file line number Diff line number Diff line change
@@ -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)
145 changes: 145 additions & 0 deletions src/Robot_arm_motion/scripts/robot_arm.py
Original file line number Diff line number Diff line change
@@ -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 = """
<mujoco>
<option gravity="0 0 -9.81" timestep="0.001"/>
<worldbody>
<!-- 地面 -->
<geom name="ground" type="plane" size="5 5 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>
<!-- 机械臂基座 -->
<body name="base" pos="0 0 0">
<geom type="cylinder" size="0.1 0.1" rgba="0.2 0.2 0.8 1"/>
<joint name="joint1" type="hinge" axis="0 1 0" pos="0 0 0.1"/>
<body name="link1" pos="0 0 0.1">
<geom type="capsule" size="0.05" fromto="0 0 0 0 0 0.5" rgba="0.2 0.8 0.2 1"/>
<joint name="joint2" type="hinge" axis="1 0 0" pos="0 0 0.5"/>
<body name="link2" pos="0 0 0.5">
<geom type="capsule" size="0.05" fromto="0 0 0 0 0 0.4" rgba="0.2 0.8 0.2 1"/>
<!-- 夹爪基座 -->
<body name="gripper_base" pos="0 0 0.4">
<geom type="box" size="0.02 0.08 0.02" pos="0 0 0" rgba="0.5 0.5 0.5 1"/>
<!-- 左夹爪 -->
<body name="left_gripper" pos="-0.04 0 0">
<joint name="left_gripper_joint" type="slide" axis="1 0 0" pos="0 0 0"/>
<geom name="left_gripper_geom" type="box" size="0.04 0.02 0.02" pos="0 0 0" rgba="0.8 0.2 0.2 1"/>
</body>
<!-- 右夹爪 -->
<body name="right_gripper" pos="0.04 0 0">
<joint name="right_gripper_joint" type="slide" axis="1 0 0" pos="0 0 0"/>
<geom name="right_gripper_geom" type="box" size="0.04 0.02 0.02" pos="0 0 0" rgba="0.8 0.2 0.2 1"/>
</body>
</body>
</body>
</body>
</body>
<!-- 抓取目标:立方体(添加唯一geom名称) -->
<body name="cube" pos="0.3 0 0.5">
<joint type="free"/>
<geom name="cube_geom" type="box" size="0.05 0.05 0.05" rgba="0.8 0.8 0.2 1"/>
</body>
</worldbody>
<actuator>
<!-- 机械臂关节电机 -->
<motor joint="joint1" ctrlrange="-3.14 3.14" gear="100"/>
<motor joint="joint2" ctrlrange="-3.14 3.14" gear="100"/>
<!-- 夹爪关节电机 -->
<motor joint="left_gripper_joint" ctrlrange="0 0.06" gear="50"/>
<motor joint="right_gripper_joint" ctrlrange="-0.06 0" gear="50"/>
</actuator>
</mujoco>
"""
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()
Empty file added src/Robot_arm_motion/test.py
Empty file.
Loading