Skip to content
Merged
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
26 changes: 19 additions & 7 deletions src/Robot_arm_motion/robot_arm.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
print(f"❌ 场景文件不存在:{SCENE_PATH}")
sys.exit(1)


# ========== 智能抓取控制器 ==========
class PandaAutoGrab:
def __init__(self):
Expand Down Expand Up @@ -50,16 +51,20 @@ def __init__(self):
self.safe_lift_height = 0.15
self.grab_height = 0.05

# PD控制参数
# 【优化1】提取PD控制参数为类内常量
self.PD_KP = 250 # 比例增益
self.PD_KD = 100 # 微分增益
self.TORQUE_LIMIT = 20 # 力矩限制

# 【优化1】提取雅克比伪逆的阻尼系数为类内常量
self.JACOBIAN_DAMPING = 0.01 # 雅克比伪逆的阻尼系数

# 打印模型信息
print("="*50)
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)
print("=" * 50)

def get_ee_pos(self):
"""获取末端执行器位置"""
Expand All @@ -70,7 +75,11 @@ def get_cube_pos(self):
return self.data.xpos[self.cube_body_id].copy()

def _compute_jacobian(self):
"""计算雅克比矩阵"""
"""计算末端执行器的位置雅克比矩阵

Returns:
np.ndarray: 3×7的位置雅克比矩阵(仅包含机械臂7个关节的分量)
"""
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]

Expand All @@ -88,8 +97,8 @@ def _move_step(self, target, tol=0.003, speed=0.3):

# ========== 修正:正确的阻尼伪逆计算 ==========
# 方法1:使用正则化参数的伪逆(推荐)
lambda_ = 0.01 # 阻尼系数
jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + lambda_ * np.eye(3))
# 【优化2】使用类内常量替代硬编码的阻尼系数
jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + self.JACOBIAN_DAMPING * np.eye(3))

# 方法2:若方法1仍报错,可改用numpy伪逆(自动处理维度)
# jacobian_pinv = np.linalg.pinv(jacobian, rcond=1e-3)
Expand Down Expand Up @@ -215,9 +224,9 @@ def _grab_phase_machine(self):
elif self.current_phase == 12:
# 阶段12:抓取完成
if not self.grab_complete:
print("\n" + "="*50)
print("\n" + "=" * 50)
print("✅ 智能抓取任务完成!")
print("="*50)
print("=" * 50)
self.grab_complete = True

def run(self):
Expand All @@ -233,6 +242,7 @@ def run(self):
print("💡 关闭Viewer窗口可退出程序")

# 提取休眠时间为常量,便于后续调整
SIMULATION_SLEEP = 1 / 200
SIMULATION_SLEEP = 1/200

# 单线程主循环
Expand All @@ -257,6 +267,7 @@ def run(self):
self.viewer.close()
print("\n👋 仿真结束")


# ========== 主函数 ==========
if __name__ == "__main__":
try:
Expand All @@ -265,5 +276,6 @@ def run(self):
except Exception as e:
print(f"\n❌ 程序错误:{e}")
import traceback

traceback.print_exc()
sys.exit(1)
Loading