MuJoCo 仿真进阶：动态可视化元素与随机障碍物添加实战

以下文章来源于机器人开发圈，作者LitchiCheng

Mujoco 动态添加可视化元素

Mujoco 中的 geom 可以用很多用处，其中如果不做碰撞（碰撞计算等mujoco也不能动态插入），实际上提供了单独的接口可以动态插入可视化的geom，这样可以很方便的显示目标点或者坐标系等等。

区别于障碍物 geom 的定义不指定 contype、conaffinity即可，这样渲染时只是显示，而不会参加碰撞计算（不然运动过程中会阻挡等），修改xml也可以达到效果，但不是动态的，如下xml

<geom name="test1" type="sphere" size="0.060" pos="0.300 0.200 0.500" rgba="0.300 0.300 0.300 0.800" />

动态接口

self.handle.user_scn.ngeommujoco.mjv_initGeom(    self.handle.user_scn.geoms[i+now_user_geom_num],    type = ob_type,    size = size,     pos=pos,    mat=np.eye(3).flatten(),    rgba=rgba)

下面的测试代码会随机生成指定数量的 geom，然后可以按down键动态添加，注意不要一直按（电脑不行会很卡，渲染也吃性能）

import mujoco_viewerimport timeimport mujocoimport numpy as npimport randomimport src.key_listener as key_listenerfrom pynput import keyboard

key_states = {    keyboard.Key.down: False,}

class Env(mujoco_viewer.CustomViewer):    def __init__(self, path, num_geoms=5):        super().__init__(path, 3, azimuth=-45, elevation=-30)        self.path = path        self.num_geoms = num_geoms        self.workspace = {            'x': [-0.3, 0.3],            'y': [-0.3, 0.3],            'z': [0.0, 0.7]        }        self.key_listener = key_listener.KeyListener(key_states)        self.key_listener.start()

  def get_random_position(self):        """在工作空间内生成随机位置"""        x = random.uniform(self.workspace['x'][0], self.workspace['x'][1])        y = random.uniform(self.workspace['y'][0], self.workspace['y'][1])        z = random.uniform(self.workspace['z'][0], self.workspace['z'][1])        return np.array([x, y, z])

  def runBefore(self):        self.usr_geom_size = []        self.usr_geom_pos = []        self.usr_geom_rgba = []        self.usr_geom_type = []        for i in range(self.num_geoms):            pos = self.get_random_position()            rgba = np.random.rand(4)            rgba[3] = 0.8            self.usr_geom_pos.append([pos[0], pos[1], pos[2]])            self.usr_geom_type.append("sphere")            self.geom_size = 0.05            self.usr_geom_size.append([self.geom_size])            self.usr_geom_rgba.append([rgba[0], rgba[1], rgba[2], 0.8])        self.addVisuGeom(self.usr_geom_pos, self.usr_geom_type, self.usr_geom_size, self.usr_geom_rgba)

  def runFunc(self):        if key_states[keyboard.Key.down]:            tmp_size = []            tmp_pos = []            tmp_rgba = []            tmp_type = []            pos = self.get_random_position()            rgba = np.random.rand(4)            rgba[3] = 0.8            tmp_pos.append([pos[0], pos[1], pos[2]])            tmp_type.append("sphere")            self.geom_size = 0.05            tmp_size.append([self.geom_size])            tmp_rgba.append([rgba[0], rgba[1], rgba[2], 0.8])            self.addVisuGeom(tmp_pos, tmp_type, tmp_size, tmp_rgba)        time.sleep(0.01)

if __name__ == "__main__":    env = Env("./model/franka_emika_panda/scene.xml", num_geoms=10)    env.run_loop()

Mujoco 随机添加碰撞物体（位置、类型、大小、颜色）以及注意点

前面介绍过使用 OMPL 库进行路径规划绕障，那么在 Mujoco 中障碍物怎么添加，有哪些，需要注意什么事项？

打开 xml 可以看下 worldbody 下定义了地板 floor

那么障碍物怎么定义，同样我们也需要定义name、size、type，除此之外还可以指定 rgba、contype、conaffinity、mass等

<geom name="obstacle_0" type="sphere" size="0.060" pos="0.300 0.200 0.500" contype="1" conaffinity="1" mass="0.0" rgba="0.300 0.300 0.300 0.800" />

其中contype和conaffinity必须为1，不然碰撞不起效，也就是data.ncon不会计算该geom，下面是生成随机障碍物，指定数量的测试代码：

import mujoco_viewerimport timeimport mujocoimport numpy as npimport random

class Env(mujoco_viewer.CustomViewer):    def __init__(self, path, num_obstacles=5):        super().__init__(path, 3, azimuth=-45, elevation=-30)        self.path = path        self.num_obstacles = num_obstacles        self.workspace = {            'x': [-0.3, 0.3],            'y': [-0.3, 0.3],            'z': [0.0, 0.7]        }        self.usr_obstacle_size = []        self.usr_obstacle_pos = []        self.usr_obstacle_rgba = []        self.usr_obstacle_type = []        for i in range(self.num_obstacles):            pos = self.get_random_position()            rgba = np.random.rand(4)            rgba[3] = 0.8            self.usr_obstacle_pos.append([pos[0], pos[1], pos[2]])            self.usr_obstacle_type.append("sphere")            self.obstacle_size = 0.05            self.usr_obstacle_size.append([self.obstacle_size])            self.usr_obstacle_rgba.append([rgba[0], rgba[1], rgba[2], 0.8])        self.addObstacles(self.usr_obstacle_pos, self.usr_obstacle_type, self.usr_obstacle_size, self.usr_obstacle_rgba)

  def get_random_position(self):        """在工作空间内生成随机位置"""        x = random.uniform(self.workspace['x'][0], self.workspace['x'][1])        y = random.uniform(self.workspace['y'][0], self.workspace['y'][1])        z = random.uniform(self.workspace['z'][0], self.workspace['z'][1])        return np.array([x, y, z])

  def runBefore(self):        pass

  def runFunc(self):        time.sleep(0.01)

if __name__ == "__main__":    env = Env("./model/franka_emika_panda/scene.xml", num_obstacles=10)    env.run_loop()

Mujoco 蒙特卡洛采样统计机械臂可达工作空间（非Matlab）

前面总是提到机械臂的可达空间，那怎么计算，有什么公式吗?

使用蒙特卡洛采样，通过大样本概率统计替代精确推导，生成随机分布的关节空间（限制关节范围），再进行正运动学 fk 即可求得机械臂末端的可达空间

self.ee_body_name = "ee_center_body"self.kine = kdl_ik.Kinematics(self.ee_body_name)self.kine.buildFromURDF(self.urdf_path, "link0")workspace = []for _ in range(self.sample_num):q = PyKDL.JntArray(self.model.nq)for i in range(self.model.nq):    q[i] = np.random.uniform(self.model.jnt_range[i][0], self.model.jnt_range[i][1])# fkmat = self.kine.fk(q)x, y, z, _, _, _ = utils.mat2transform(mat)workspace.append([x, y, z])

最后将x,y,z的集合通过散点图画出来，同时打印下分别的最大最小值，但这个值通常也不是无脑使用，除非只关心某一个维度，不然还是要一起考虑，一般控制前可以通过ik是否可以得到解来判断是否最终控制是有效的。

完整代码如下：

import src.kdl_kinematic as kdl_kinematicimport PyKDLimport numpy as npimport osimport mujoco_viewerimport mujocoimport utils

import matplotlib.pyplot as pltfrom mpl_toolkits.mplot3d import Axes3D

class GetWorkspace(mujoco_viewer.CustomViewer):    def __init__(self, scene_path, urdf_path, sample_num = int(10e4)):        super().__init__(scene_path, 3, azimuth=-45, elevation=-30)        self.path = scene_path        self.urdf_path = urdf_path        self.sample_num = sample_num

  def runBefore(self):        self.ee_body_name = "ee_center_body"        self.kine = kdl_kinematic.Kinematics(self.ee_body_name)        self.kine.buildFromURDF(self.urdf_path, "link0")        workspace = []        for _ in range(self.sample_num):            q = PyKDL.JntArray(self.model.nq)            for i in range(self.model.nq):                q[i] = np.random.uniform(self.model.jnt_range[i][0], self.model.jnt_range[i][1])            # fk            mat = self.kine.fk(q)            x, y, z, _, _, _ = utils.mat2transform(mat)            workspace.append([x, y, z])        self.plotWorkspace(np.array(workspace))

  def plotWorkspace(self, workspace):        # X轴        x_max = workspace[:, 0].max()        x_min = workspace[:, 0].min()        x_max_idx = np.argmax(workspace[:, 0])        x_min_idx = np.argmin(workspace[:, 0])        x_max_point = workspace[x_max_idx]        x_min_point = workspace[x_min_idx]

      # Y轴        y_max = workspace[:, 1].max()        y_min = workspace[:, 1].min()        y_max_idx = np.argmax(workspace[:, 1])        y_min_idx = np.argmin(workspace[:, 1])        y_max_point = workspace[y_max_idx]        y_min_point = workspace[y_min_idx]

      # Z轴        z_max = workspace[:, 2].max()        z_min = workspace[:, 2].min()        z_max_idx = np.argmax(workspace[:, 2])        z_min_idx = np.argmin(workspace[:, 2])        z_max_point = workspace[z_max_idx]        z_min_point = workspace[z_min_idx]

      print(f"X轴：最大值 {x_max:.4f} m | 最小值 {x_min:.4f} m")        print(f"Y轴：最大值 {y_max:.4f} m | 最小值 {y_min:.4f} m")        print(f"Z轴：最大值 {z_max:.4f} m | 最小值 {z_min:.4f} m")

      fig = plt.figure(figsize=(12, 10))        ax = fig.add_subplot(111, projection='3d')        ax.scatter(workspace[:,0], workspace[:,1], workspace[:,2], s=1, alpha=0.5, c='lightblue', label='workspace')

      ax.scatter(x_max_point[0], x_max_point[1], x_max_point[2],                 s=300, c='red', marker='*', label=f'X Maximum ({x_max:.4f} m)')        ax.scatter(x_min_point[0], x_min_point[1], x_min_point[2],                 s=300, c='red', marker='d', label=f'X Minimum ({x_min:.4f} m)')

      ax.scatter(y_max_point[0], y_max_point[1], y_max_point[2],                 s=300, c='green', marker='*', label=f'Y Maximum ({y_max:.4f} m)')        ax.scatter(y_min_point[0], y_min_point[1], y_min_point[2],                 s=300, c='green', marker='d', label=f'Y Minimum ({y_min:.4f} m)')

      ax.scatter(z_max_point[0], z_max_point[1], z_max_point[2],                 s=300, c='blue', marker='*', label=f'Z Maximum ({z_max:.4f} m)')        ax.scatter(z_min_point[0], z_min_point[1], z_min_point[2],                 s=300, c='blue', marker='d', label=f'Z Minimum ({z_min:.4f} m)')

      ax.set_xlabel('X (m)', fontsize=12)        ax.set_ylabel('Y (m)', fontsize=12)        ax.set_zlabel('Z (m)', fontsize=12)        ax.set_title('workspace', fontsize=14)        ax.legend(fontsize=10)        # plt.tight_layout()        # plt.show()

  def runFunc(self):          plt.tight_layout()        plt.show(block=False)        plt.pause(0.001)

if __name__ == "__main__":    urdf_file = "model/franka_panda_urdf/robots/panda_arm.urdf"    SCENE_XML_PATH = 'model/franka_emika_panda/scene_pos.xml'    robot = GetWorkspace(SCENE_XML_PATH, urdf_file, sample_num=int(5e4))    robot.run_loop()

Mujoco 冗余机械臂零空间 Null Space 运动仿真

当描述系统输入与输出映射的矩阵（如机械臂雅可比矩阵）满足 “列数大于行数”（即输入维度＞输出维度），且矩阵列秩小于行数时，存在非零输入向量使得输出为零，这些非零输入向量的集合即为零空间。

对机械臂而言，当关节自由度（输入维度）大于末端执行器的任务自由度（输出维度，如 3 维位置 + 3 维姿态共 6 个自由度）时，雅可比矩阵列数（关节数）＞行数（任务自由度），此时必然存在零空间。

零空间实际是雅可比的空间的子空间，在这个零空间内的任何关节速度都不会影响末端，因为 Ax = 0。实际应用：机械臂可在保持末端位姿不变的前提下，关节可以在一定范围内进行调整。核心就是零空间投影，使用投影矩阵可以把期望的关节速度或者关节状态变化到零空间中，如下：

def nullSpaceProjection(self, J, vel_candidate):    # 零空间投影矩阵：P_null = I - J^+J    I = np.eye(J.shape[1])    J_pinv = np.linalg.pinv(J, rcond=1e-6)    P_null = I - np.dot(J_pinv, J)    return np.dot(P_null, vel_candidate)

本次实验可以将关节2转动，如下：

def geVelCandidate(self, q, q_mid, gain=0.1):    vel_candidate = np.zeros(self.model.nq)    vel_candidate[2] = 0.7    return vel_candidate

使用franka机械臂，起始位置使用ik生成一个末端位姿，然后让关节2进行转动，末端始终保持不动，完整代码如下：

import pinocchio import mujoco_viewerimport timeimport numpy as npimport mujocoimport utilsimport src.pinocchio_kinematic as pinocchio_kinematic

class PandaPbvs(mujoco_viewer.CustomViewer):    def __init__(self, scene_xml, pin_xml):        super().__init__(scene_xml, 3, azimuth=-45, elevation=-30)        self.scene_xml = scene_xml        self.pin_xml = pin_xml

  def runBefore(self):        self.model.opt.timestep = 0.01        self.kine = pinocchio_kinematic.Kinematics("ee_center_body")        self.kine.buildFromMJCF(self.pin_xml)        tf = utils.transform2mat(0.1, 0.0, 0.5, np.pi, 0, 0)        dof, info = self.kine.ik(tf)        self.data.qpos[:7] = dof[:7]        self.prev_joint_vel = np.zeros(9, dtype=np.float32)          self.keep_pos = False        self.integral_qpos = self.data.qpos.copy()

      self.joint_mid = np.zeros(self.model.nq)        for i in range(self.model.nq):            self.joint_mid[i] = (self.model.jnt_range[i][0] + self.model.jnt_range[i][1]) / 2

  def nullSpaceProjection(self, J, vel_candidate):        # 零空间投影矩阵：P_null = I - J^+J        I = np.eye(J.shape[1])        J_pinv = np.linalg.pinv(J, rcond=1e-6)        P_null = I - np.dot(J_pinv, J)        return np.dot(P_null, vel_candidate)

  def geVelCandidate(self, q, q_mid, gain=0.1):        vel_candidate = np.zeros(self.model.nq)        vel_candidate[2] = 0.7        return vel_candidate

  def runFunc(self):        np.set_printoptions(precision=3)        J = self.kine.getJac(self.data.qpos)        vel_candidate = self.geVelCandidate(self.data.qpos, self.joint_mid)        vel_null = self.nullSpaceProjection(J, vel_candidate)        self.integral_qpos[:7] += vel_null[:7] * self.model.opt.timestep  # 更新关节位置        for i in range(self.model.nq):            self.integral_qpos[i] = np.clip(                self.integral_qpos[i],                self.model.jnt_range[i][0],                self.model.jnt_range[i][1]            )        # print("Integral position: ", self.integral_qpos)        self.data.ctrl[:7] = self.integral_qpos[:7]

if __name__ == "__main__":    CONTROLER = "pos"    scene_xml_path = "model/franka_emika_panda/scene_"+ CONTROLER + ".xml"    pin_xml_path = "model/franka_emika_panda/panda_"+ CONTROLER + ".xml"    env = PandaPbvs(scene_xml_path, pin_xml_path)    env.run_loop()

【有奖直播报名中】2026 是德科技数字月新品发布: 一段跨越70年的示波器创新之旅

【直播时间】3月13日（周五）下午14:00-15:30

【直播看点】

1956年是德科技发布了首批示波器，它成为测量产品线的重要组成部分。跨越70年，2026年, 下一个新品即将发布！

本次发布会中，是德科技专家将结合实际工程经验，剖析当前数字验证中常见的效率瓶颈，解析哪些环节正在无形中拉长测试周期、影响项目进度。

· END ·

进群和电子工程师们面对面交流经验