MuJoCo 机械臂关节空间阻抗控制Impedance实现（附代码）
视频讲解：
MuJoCo 机械臂关节空间阻抗控制Impedance实现（附代码）
代码仓库：GitHub - LitchiCheng/mujoco-learning
阻抗控制通过调整关节的刚度和阻尼，让机器人在接触时产生顺应性，减少冲击力，其核心目标是在接触外部载荷时能够自适应调整输出力矩，实现力与位置的协同控制。比如足式机器人（如四足机器人）在复杂地形行走时，通过调整关节阻抗应对地面冲击（如踏空、打滑），增强步态稳定性和地形适应性。
实验目标为so_arm100机械臂，两个测试：
- 使用阻抗控制让关节到达预定位置
- 在预定位置进行施加和释放外力
第一步首先关掉MuJoCo的配置中的阻抗控制参数，在xml中注释掉如下
注释掉该行后，直接simulate仿真可以看到机械臂控制位置震荡等情况，接下来引入关节空间阻抗控制的代码
# 读取当前关节角度和速度
q = self.data.qpos[:self.model.nu]
qdot = self.data.qvel[:self.model.nu]
# 计算阻抗控制扭矩
error = self.q_desired - q
print(self.index, self.num_steps, self.model.nu, error)
torque = self.Kp @ error - self.Kd @ qdot
# 设置控制输入
self.data.ctrl[:] = torque其中self.Kp和self.Kd为固定刚度和阻尼系数，通过修改刚度和阻尼参数，可以变得更柔顺来应对各种场景
self.Kp = np.diag([100] * self.model.nu) # 刚度矩阵
self.Kd = np.diag([10] * self.model.nu) # 阻尼矩阵设定一个目标位置
self.q_desired = [0.0, -0.991, 0.196, 0.662, -0.88, 0.66]阻抗控制可以很快的到达设定位置，在此基础上，使用mujoco交互中的施加外力操作，可以看到当外力松开后，机械臂也可以很快的回弹回设定位置
前面一段为从关节0位到达预定位置，后面一段为在关节位置施加外力
完整代码：
import mujoco
import numpy as np
import matplotlib.pyplot as plt
import time
import mujoco_viewer
class Test(mujoco_viewer.CustomViewer):
def __init__(self, path):
super().__init__(path, 1, azimuth=45, elevation=-30)
self.path = path
def runBefore(self):
# 阻抗控制参数
self.Kp = np.diag([100] * self.model.nu) # 刚度矩阵
self.Kd = np.diag([10] * self.model.nu) # 阻尼矩阵
# 目标关节角度
# self.q_desired = np.zeros(self.model.nu)
self.q_desired = [0.0, -0.991, 0.196, 0.662, -0.88, 0.66]
# 仿真参数
self.total_time = 30 # 总仿真时间（秒）
self.dt = self.model.opt.timestep # 仿真时间步长
self.num_steps = int(self.total_time / self.dt)
# 存储数据
self.q_history = np.zeros((self.num_steps, self.model.nu))
self.qdot_history = np.zeros((self.num_steps, self.model.nu))
self.torque_history = np.zeros((self.num_steps, self.model.nu))
self.index = 0
def runFunc(self):
# 读取当前关节角度和速度
q = self.data.qpos[:self.model.nu]
qdot = self.data.qvel[:self.model.nu]
# 计算阻抗控制扭矩
error = self.q_desired - q
print(self.index, self.num_steps, self.model.nu, error)
torque = self.Kp @ error - self.Kd @ qdot
# 设置控制输入
self.data.ctrl[:] = torque
if False:
self.q_history[self.index] = q
self.qdot_history[self.index] = qdot
self.torque_history[self.index] = torque
self.index += 1
if self.index >= self.num_steps:
# # 绘制结果
time = np.arange(0, self.total_time, self.dt)
plt.figure(figsize=(12, 8))
# 绘制关节角度
plt.subplot(3, 1, 1)
for j in range(self.model.nu):
plt.plot(time, self.q_history[:, j], label=f'Joint {j+1}')
plt.title('Joint Angles')
plt.xlabel('Time (s)')
plt.ylabel('Angle (rad)')
plt.legend()
# 绘制关节速度
plt.subplot(3, 1, 2)
for j in range(self.model.nu):
plt.plot(time, self.qdot_history[:, j], label=f'Joint {j+1}')
plt.title('Joint Velocities')
plt.xlabel('Time (s)')
plt.ylabel('Velocity (rad/s)')
plt.legend()
# 绘制控制扭矩
plt.subplot(3, 1, 3)
for j in range(self.model.nu):
plt.plot(time, self.torque_history[:, j], label=f'Joint {j+1}')
plt.title('Control Torques')
plt.xlabel('Time (s)')
plt.ylabel('Torque (N.m)')
plt.legend()
plt.tight_layout()
plt.show()
test = Test("./model/trs_so_arm100/scene.xml")
test.run_loop()