Mujoco 实用技术集：MJCF/URDF 手动对齐+Body 位姿获取 + KDL 运动学控制

Mujoco 模型 MJCF 和 URDF 如何手动对齐（Pinocchio验证）

原贴链接：

https://www.eeworld.com.cn/aOO0qfL

代码仓库：

https://github.com/LitchiCheng/mujoco-learning

在Mujco中的模型为mjcf的xml，其他仿真ROS2中，urdf也是xml文件，通常可以通过一些工具进行互相转换，但转换的结果却经常不正确，有几种情况：

模型直接运行不了，报错

模型的mesh文件丢失，显示不对

模型的link和body等对应不上

所以手动修改MJCF和URDF作为保底手段，还是要理解哪些是重要部分和一定要对齐的地方，前提是这两份模型的差异不是很大（手动狗头，不然改到吐），还有就是理解其中要对齐的方向是什么（是否只考虑运动学等等）。

下面介绍下其中几个关键的字段：

MCJF中的Body

再者就是位置关系，如MCJF中位置定义在body字段中，而URDF定义在link之间的joint，如下以添加一个新的link和body为例，MJCF添加新的body（ee_center_body），位置为pos="0 0 0.105，姿态为quat="1 0 0 0"

URDF添加新的link，同时添加joint，位置为 xyz="0 0 0.105"，姿态为rpy="0 0 0"

<link name="ee_center_body">    <inertial>      <origin rpy="0 0 0" xyz="0 0 0"/>      <mass value="0.0"/>      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>    </inertial>  </link>  <joint name="joint9" type="fixed">    <origin rpy="0 0 0" xyz="0 0 0.105"/>    <parent link="hand"/>    <child link="ee_center_body"/>    <axis xyz="0 0 0"/>  </joint>

下面使用pinocchio进行验证

frompathlibimportPathfrom sysimportargv importpinocchioaspinimport numpyasnp   deftest_urdf_match_with_mjcf(model):    print("model name: " + model.name)    print("lowerLimits: " + str(model.lowerPositionLimit))    print("upperLimits: " + str(model.upperPositionLimit))     data = model.createData()    q_list = [ 1.97125175, -0.37236355, 1.64044676, -0.67488302, 2.38533178, 0.72726866, -0.95481822]    q = np.array(q_list, dtype=np.float64)  # Perform the forward kinematics over the kinematic tree    pin.forwardKinematics(model, data, q)    # Print out the placement of each joint of the kinematic tree    for name, oMi in zip(model.names, data.oMi):        print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat)) if__name__ == '__main__':    print("mjcf test")    model_mjcf = pin.RobotWrapper.BuildFromMJCF("model/franka_emika_panda/panda_pos.xml").model    test_urdf_match_with_mjcf(model_mjcf)    print("")    print("urdf test")    model_urdf = pin.buildModelFromUrdf("model/franka_panda_urdf/robots/panda_arm.urdf")    test_urdf_match_with_mjcf(model_urdf)

model.nq需要保持一致，mjcf多了两个finger，删除finger也可以，或者传入qpos的时候创建9x1的维度即可。

Mujoco 基础获取模型中所有 body 的 name, id 以及位姿

原贴链接：

https://www.eeworld.com.cn/aHuL4CS

代码仓库：

https://github.com/LitchiCheng/mujoco-learning

今天介绍下一个很常用的接口：获取模型中所有的 body 的 name，id，以及位姿。

经常做测试时会遇到获取某个 body 的位姿以及利用该位姿，我们可以通过指定 body，先获取 id，再通过 id 获取 ee 的世界坐标系下的位姿，然后将位置作为对运动学的一个目标，同时可以在模型当中添加一些可视化的元素，更直观的来看自己的代码或者说这个实验是否达到预期，如下为常用来的接口或字段：

body的数量

model.nbody

通过id获取名称

mujoco.mj_id2name

获取父 id

model.body_parentid

获取指定 id 的位置

data.body(body_id).xpos

如下代码为获取所有body的id、以及位姿

importmujocoimport timeimport mujoco_viewerimport numpyasnp classTest(mujoco_viewer.CustomViewer):  def__init__(self, path):        super().__init__(path, 3, azimuth=-45, elevation=-30)        self.path = path   defrunBefore(self):      forbody_idinrange(self.model.nbody):          # 参数说明：model=模型，obj_type=对象类型（body），obj_id=body ID            body_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_BODY, body_id)            # 获取父 body ID            parent_body_id = self.model.body_parentid[body_id]            # print(f"{body_id:<10} {body_name:<20} {parent_body_id:<15}")             pos = self.data.body(body_id).xpos            quat = self.data.body(body_id).xquat            print(f"id:{body_id}, name: {body_name}, Position: {pos}, Quaternion: {quat}")   defrunFunc(self):        placeholder = 0 test = Test("./model/franka_emika_panda/scene_pos.xml")test.run_loop()

也可以通过在viewer中显示body看到实际的位置。

Mujoco 使用KDL 对机械臂进行 IK 和 FK 运动学控制末端移动

原贴链接：

https://www.eeworld.com.cn/avzfPaL

代码仓库：

https://github.com/LitchiCheng/mujoco-learning

前面介绍过通过 UV 进行安装PyKDL的方法以及一些注意事项，今天我们用 KDL 来进行机械臂IK的测试，来控制 Franka机械臂的末端 ee_center_body 沿着 X 方向进行移动，需要注意的是，Mujoco显示需要使用xml，KDL目前只支持URDF，可以参考我的其他分享，如何将MJCF和URDF手动对齐。

需要提前创建好各个求解器，求解器也有很多种，这里使ChainIkSolverPos_NR，需要设置迭代次数和收敛精度阈值。

defcreateSolver(self):      self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.chain)      self.ik_vel_solver = PyKDL.ChainIkSolverVel_pinv(self.chain)        max_iterations = 500        eps = 1e-6      self.ik_pos_solver = PyKDL.ChainIkSolverPos_NR(          self.chain,self.fk_solver,self.ik_vel_solver, max_iterations, eps        )      self.jac_solver = PyKDL.ChainJntToJacSolver(self.chain)

如下为ik函数，可以传入当前关节位置，防止关节突变等，传入的tf为4x4的齐次变换矩阵的位姿。

   def ik(self, tf , current_arm_motor_q = None, current_arm_motor_dq = None):        num_joints =self.chain.getNrOfJoints()      ifcurrent_arm_motor_q is None:            q_init = PyKDL.JntArray(num_joints)      else:            q_init = PyKDL.JntArray(num_joints)          foriinrange(num_joints):                q_init[i] = current_arm_motor_q[i]        q_out = PyKDL.JntArray(num_joints)        ## 提取平移向量（前3行第4列）        trans = PyKDL.Vector(tf[0, 3], tf[1, 3], tf[2, 3])        ## 提取旋转矩阵（前3行前3列），构造PyKDL.Rotation        # Rotation构造参数：xx, xy, xz, yx, yy, yz, zx, zy, zz（行优先展开）        rot = PyKDL.Rotation(            tf[0, 0], tf[0, 1], tf[0, 2],            tf[1, 0], tf[1, 1], tf[1, 2],            tf[2, 0], tf[2, 1], tf[2, 2]        )        frame = PyKDL.Frame(rot, trans)        dof = []        status =self.ik_pos_solver.CartToJnt(q_init, frame, q_out)      ifstatus >= 0:            dof = [q_out[i]foriinrange(num_joints)]            info = {"success": True}      else:            dof = q_init            info = {"success": False, "error_code": status}      returndof, info

如下为fk函数，将当前关节空间映射到末端笛卡尔坐标系下的位姿。

def fk(self, q):        num_joints = self.chain.getNrOfJoints()        q_kdl = PyKDL.JntArray(num_joints)        for i inrange(num_joints):            q_kdl[i] = q[i]        end_frame = PyKDL.Frame()        self.fk_solver.JntToCart(q_kdl, end_frame)        tf = np.eye(4, dtype=np.float64)        rot_mat = np.array([            [end_frame.M[0,0], end_frame.M[0,1], end_frame.M[0,2]],            [end_frame.M[1,0], end_frame.M[1,1], end_frame.M[1,2]],            [end_frame.M[2,0], end_frame.M[2,1], end_frame.M[2,2]]        ], dtype=np.float64)        tf[:3, :3] = rot_mat        tf[:3, 3] = np.array([end_frame.p.x(), end_frame.p.y(), end_frame.p.z()])        return tf

如下为完整的测试代码，需要注意如上代码封装在kdl_ik中，需要import进来，另外注意URDF中是否包含对应Link name，最后通过打印Mujoco的位姿以及fk后的位姿，可以看到位置接近（这里mujoco的body pos和urdf应该有offset）

import mujocoimport numpyasnpimport mujoco_viewerimport src.kdl_ikaskdl_ikimport timeimport osimport utils class RobotController(mujoco_viewer.CustomViewer):        def __init__(self, scene_path, arm_path):      super().__init__(scene_path, distance=3.5, azimuth=180, elevation=-90)      self.scene_path = scene_path      self.arm_path = arm_path       self.ee_body_name = "ee_center_body"        # 初始化逆运动学      self.arm = kdl_ik.Kinematics(self.ee_body_name)        urdf_file = "model/franka_panda_urdf/robots/panda_arm.urdf"      self.arm.buildFromURDF(urdf_file, "link0")      self.end_effector_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY,self.ee_body_name)      self.last_dof = None     def runBefore(self):      self.model.opt.timestep = 0.001        # 设定初始位置      self.initial_pos =self.model.key_qpos[0]          print("Initial position: ",self.initial_pos)      foriinrange(self.model.nq):          self.data.qpos[i] =self.initial_pos[i]        # mujoco.mj_forward(self.model, self.data)        ee_pos =self.data.body(self.end_effector_id).xpos.copy()        # self.x = ee_pos[0]        # self.y = ee_pos[1]        # self.z = ee_pos[2]      self.x = 0.1      self.y = 0.1      self.z = 0.5      self.last_dof =self.data.qpos[:9].copy()     def runFunc(self):      self.x += 0.001      ifself.x > 0.5:          self.x = 0.5          self.data.qpos[:7] =self.last_dof[:7]          self.data.qpos[7:] = [0,0]      else:            tf = utils.transform2mat(self.x,self.y,self.z, np.pi, 0, 0)          self.dof, info =self.arm.ik(tf, current_arm_motor_q=self.last_dof)          ifinfo["success"]:              self.last_dof =self.dof              self.data.qpos[:7] =self.dof[:7]              self.data.qpos[7:] = [0,0]                # self.data.ctrl[:7] = self.dof[:7]                print("ee pos",self.getBodyPosByName(self.ee_body_name))                print("fk",self.arm.fk(self.dof[:7]))        time.sleep(0.01) if__name__ == "__main__":    SCENE_XML_PATH = 'model/franka_emika_panda/scene_pos.xml'    ARM_XML_PATH = 'model/franka_emika_panda/panda_pos.xml'    robot = RobotController(SCENE_XML_PATH, ARM_XML_PATH)    robot.run_loop()