MuJoCo 机械臂仿真进阶：从MPC模型预测控制到传感器数据应用

在《Mujoco 机械臂进行 PBVS 基于位置的视觉伺服思路》中本质使用的是PD控制进行位置闭环，其中通过clip进行截断最终下发速度来防止超限等行为，属于对计算结果的处理，如下

q_dot_max = 3.14
q_dot_des = np.clip(q_dot_core, -q_dot_max, q_dot_max)  # 限幅后关节速度

对于连续性和实际的运行来说并不是最佳方案，MPC 的通过滚动时域及约束的控制思路可以避免该现象，将速度、限位、甚至加速度等约束都可加入到计算过程中，通过优化器进行求解，最终得到的是在约束内的结果，而不需要在结果再进行截断处理。（MPC原理不再赘述，太多好的视频）

本次实验将应用 MPC 进行 PBVS 的控制，首先需要定义 MPC 控制器的基本参数，实际过程中经常需要调节这些参数里达到预期或期望的效果。

self
.N = N          # 预测时域
self
.Ts = Ts        # 控制周期
self
.Q_p = 20.0 * np.eye(3)   # 位置权重
self
.Q_r = 5.0  * np.eye(3)   # 旋转权重
self
.R = 0.001 * np.eye(self.joints_num)  # 速度惩罚
self
.q_min = self.mjcf_model.lowerPositionLimit
self
.q_max = self.mjcf_model.upperPositionLimit
self
.dq_min = np.array([-6.175]*self.joints_num)
self
.dq_max = np.array([ 6.175]*self.joints_num)

N 往前看多少时间，越长计算量大，CPU消耗会增加，Ts 一般离散控制的实际周期，Q/R 代价函数中的惩罚权重，q_min/max 等为约束（subject to）

构建优化目标和用于CasAdi的符号运算

# 符号变量

self
.q_sym = ca.SX.sym("q", self.joints_num, 1)
self
.ref_tf_sym = ca.SX.sym("ref_tf", 4, 4)

# 正运动学

cpin.framesForwardKinematics(self.cmodel, self.cdata, self.q_sym)

# 位置误差函数

self
.translational_error = ca.Function(
"translational_error"
,
[self.q_sym, self.ref_tf_sym],
[self.cdata.oMf[self.ee_id].translation - self.ref_tf_sym[:3, 3]]
)

# 旋转误差函数

self
.rotational_error = ca.Function(
"rotational_error"
,
[self.q_sym, self.ref_tf_sym],
[cpin.log3(self.cdata.oMf[self.ee_id].rotation @ self.ref_tf_sym[:3,:3].T)]
)

self
.opti = ca.Opti()

# 预测时域内的关节速度 dq[0...N-1]

self
.var_dq = self.opti.variable(self.joints_num, self.N)

# 当前关节角、目标位姿

self
.param_q0 = self.opti.parameter(self.joints_num)
self
.param_ref_tf = self.opti.parameter(4, 4)

total_cost = 0
q_prev = self.param_q0

for
 k in range(self.N):
dq_k = self.var_dq[:, k]
q_k = q_prev + dq_k * self.Ts

e_p = self.translational_error(q_k, self.param_ref_tf)
e_r = self.rotational_error(q_k, self.param_ref_tf)

total_cost += ca.bilin(self.Q_p, e_p, e_p)
total_cost += ca.bilin(self.Q_r, e_r, e_r)
total_cost += ca.bilin(self.R, dq_k, dq_k)

self
.opti.subject_to(self.q_min <= q_k)
self
.opti.subject_to(q_k <= self.q_max)
self
.opti.subject_to(self.dq_min <= dq_k)
self
.opti.subject_to(dq_k <= self.dq_max)

q_prev = q_k

优化目标为如下，u就是关节速度，过程中判断末端到位误差

在循环中将当前关节位置和目标更新，最终得到当前时域下最优的控制序列，取第一个作为下发，调用代码如下

classPandaPbvs(mujoco_viewer.CustomViewer):

def__init__(self, scene_xml, calc_xml):

super().__init__(scene_xml, 3, azimuth=-45, elevation=-30)
self.scene_xml = scene_xml
self.model.opt.timestep = 0.005
self.mpc = PbvsMpc(calc_xml, frame_name="ee_center_body", N=3, Ts=self.model.opt.timestep)
self.joints_num = self.model.nq
self.plot_manager = mp.MultiChartRealTimePlotManager()
for
 i in range(self.joints_num):
self.plot_manager.addNewFigurePlotter("j"+str(i), "j"+str(i)+"_vel", row=i, col=0)

defrunBefore(self):

self.initial_pos = self.model.key_qpos[0]  
print("Initial position: ", self.initial_pos)
for
 i in range(self.model.nq):
self.data.qpos[i] = self.initial_pos[i]

# 给一个虚拟位置

self.T_object = np.eye(4)
self.T_object[:3,3] = [0.5, 0.1, 0.2]
roll = np.pi
pitch = 0.0
yaw = 0.0
R_des = utils.euler2rotmat(roll, pitch, yaw)
self.T_object[:3, :3] = R_des

defrunFunc(self):

dp_cmd, ok = self.mpc.solve(self.T_object, self.data.qpos, self.data.qvel)
if
 ok:
# print(dp_cmd)

for
 i in range(self.joints_num):
self.plot_manager.updateDataToPlotter("j"+str(i), "j"+str(i)+"_vel", dp_cmd[i])
self.data.ctrl[:7] = dp_cmd[:7]
else
:
self.data.ctrl[:7] = np.zeros(7)
print(self.getBodyPositionByName("ee_center_body"))

在 Mujoco 中进行 RL 或者仿真，针对一个body或者一些关节甚至是交互数据，如力、速度等，部分可以通过计算或者直接读取 data 获得，但 Mujoco 已经提供了较为方便的传感器类型及方式。

1. mjData->sensordata 传感器数据数组，存储所有仿真传感器的当前读数
2. 内置多种传感器，其数据都会存储在 sensordata 中：
1. 关节位置/速度传感器（JOINTPOS, JOINTVEL）
2. 陀螺仪/加速度计（GYRO, ACCELEROMETER）
3. 力/力矩传感器（FORCE, TORQUE）
4. 触摸传感器（TOUCH）
5. 磁场传感器（MAGNETOMETER）
6. 摄像头/深度传感器（通过 CAMERA 相关机制）
7. 用户自定义传感器（USER）

更具体传感器及xml参考，可以 refer

https://mujoco.readthedocs.io/en/stable/XMLreference.html#sensor

修改下mujoco-learning工程中 model/franka_emika_panda/panda_vel.xml，增加一个sensor记录下joint1的关节速度

<sensor>

<jointvelname="joint1_vel"joint="joint1"/>

</sensor>

测试的代码，通过控制关节1的变化，同时监控关节1的传感器数据，以及使用plot进行在线绘图观察

import
 src.mujoco_viewer as mujoco_viewer
import
 src.matplot as mp

classPandaEnv(mujoco_viewer.CustomViewer):

def__init__(self, path):

super().__init__(path, 3, azimuth=-45, elevation=-30)
self.path = path

defgetSensorDataByName(self, sensor_name):

sensor_id = self.model.sensor(sensor_name).id
sensor_type = self.model.sensor(sensor_name).type
sensor_dim = self.model.sensor(sensor_name).dim[0]
# 获取数据在数组中的起始位置

adr = self.model.sensor_adr[sensor_id]
# 读取相应维度的数据

sensor_values = self.data.sensordata[adr:adr+sensor_dim]
return
 sensor_values, sensor_type, sensor_dim

defrunBefore(self):

self.initial_pos = self.model.key_qpos[0].copy()
for
 i in range(self.model.nq):
self.data.qpos[i] = self.initial_pos[i]
self.plot_manager = mp.MultiChartRealTimePlotManager()
self.plot_manager.addNewFigurePlotter("j1_sensor_vel", "j1_sensor_vel", row=0, col=0)
self.plot_manager.addNewFigurePlotter("j1_qvel", "j1_qvel", row=1, col=0)

defrunFunc(self):

current_joint1_vel,_,_ = self.getSensorDataByName("joint1_vel")
self.plot_manager.updateDataToPlotter("j1_sensor_vel", "j1_sensor_vel", current_joint1_vel[0])
self.plot_manager.updateDataToPlotter("j1_qvel", "j1_qvel", self.data.qvel[0])

if
 __name__ == "__main__":
env = PandaEnv("./model/franka_emika_panda/scene_pos.xml")
env.run_loop()

欢迎关注EEWorld旗下订阅号：“机器人开发圈”