Mujoco 使用 Pinocchio 进行逆动力学及阻抗力矩控制维持当前位置

视频讲解：https://www.bilibili.com/video/BV1nVqDBTEma/?vd_source=5ba34935b7845cd15c65ef62c64ba82f

代码仓库：https://github.com/LitchiCheng/mujoco-learning

今天介绍下机械臂高级控制，使用 pinocchio 的动力学方法 RNEA（递归牛顿-欧拉算法 Recursive Newton Euler Alogrithm）进行逆运动学，包含重力、科氏 / 离心力、惯性力三项补偿

使用的 URDF 中的惯性参数、质量质心等，参数基本正确但实际还是存在误差，故再使用阻抗进行补偿维持当前位置。

动力学补偿需要输入当前关节位置、当前关节速度、当前关节加速度，对应核心公式如下：

τ=M(q)q¨+C(q,q˙)q˙+G(q)

q 关节位置

q˙ 关节速度

q¨ 关节加速度

M 惯性力

C 科氏力、离心力

G 重力

对应在mujoco场景下，逆动力学 Tau 计算代码如下，这里期望保持当前位置不动，可以给 a 即加速度为 0（给真实值也可以，看目标期望是什么）

复制代码

q = self.data.qpos[:7]
v = self.data.qvel[:7]
a = np.zeros(7)
v = np.concatenate((v, np.zeros(2)))
q = np.concatenate((q, np.zeros(2)))
a = np.concatenate((a, np.zeros(2)))
dynamic_tau = pin.rnea(self.pin_model, self.pin_data, q, v, a)

这里存在一定误差（模型参数不准等）使得纯靠动力学无法完全维持当前位置，故可以通过阻抗控制也会拉回期望位置，阻抗控制核心如下

复制代码

 self.Kp = np.diag([100] * 7)
 self.Kd = np.diag([10] * 7)
 error = self.initial_pos - q
 impedence_tau = self.Kp @ error[:7] - self.Kd @ v[:7]

完整代码如下，2000步后打印阻抗补偿和动力学补偿的曲线图。

复制代码

import mujoco_viewer
import time
import mujoco
import numpy as np
from scipy.spatial.transform import Rotation as R
import matplotlib.pyplot as plt
import pinocchio as pin

class PandaEnv(mujoco_viewer.CustomViewer):
    def __init__(self, scene_xml, arm_xml):
        super().__init__(scene_xml, 3, azimuth=-45, elevation=-30)
        self.scene_xml = scene_xml
        self.arm_xml = arm_xml
        
        self.initial_pos = self.model.key_qpos[0]
        self.data.qpos[:7] = self.initial_pos[:7]
        self.data.qvel[:7] = np.zeros(7)

        self.step = 0
        self.step_list = []
        self.dynamics_tau_list = []
        self.damping_tau_list = []

    def runBefore(self):
        self.pin_model = pin.RobotWrapper.BuildFromMJCF(self.arm_xml).model
        self.pin_data = self.pin_model.createData()

    def runFunc(self):
        q = self.data.qpos[:7]
        v = self.data.qvel[:7]
        a = np.zeros(7)
        v = np.concatenate((v, np.zeros(2)))
        q = np.concatenate((q, np.zeros(2)))
        a = np.concatenate((a, np.zeros(2)))
        dynamics_tau = pin.rnea(self.pin_model, self.pin_data, q, v, a)

        IMPENDANCE_COMPENSATION = True
        if IMPENDANCE_COMPENSATION:
            self.Kp = np.diag([100] * 7)
            self.Kd = np.diag([10] * 7)
        else:
            self.Kp = np.diag([0] * 7)
            self.Kd = np.diag([0] * 7)
        error = self.initial_pos - q
        impedence_tau = self.Kp @ error[:7] - self.Kd @ v[:7]
        impedence_tau = np.concatenate((impedence_tau, np.zeros(2))) 
        tau = dynamics_tau + impedence_tau
        
        self.data.ctrl[:7] = tau[:7]
        print(f"Total Torque: {np.round(tau[:7], 2)}")
        print(f"impedence_tau: {np.round(impedence_tau[:7], 2)}")

        self.step += 1
        self.step_list.append(self.step)
        self.dynamics_tau_list.append(dynamics_tau[:7].copy())
        self.damping_tau_list.append(impedence_tau[:7].copy())
        # if self.step >= 2000:
        #     self.plotTorque()

if __name__ == "__main__":
    SCENE_XML = "model/franka_emika_panda/scene_tau.xml"
    ARM_XML = "model/franka_emika_panda/panda_tau.xml"
    env = PandaEnv(SCENE_XML, ARM_XML)
    env.run_loop()

维持 home 位置

维持 home 位置+在末端增加一个力扰动

维持 home 位置+调整阻抗系数

维持 home 位置+调整阻抗系数+增加末端力扰动