MuJoCo 入门教程（五）Python 绑定（上）+https://developer.aliyun.com/article/1585324
3.2.4 时间步长和发散
当我们增加时间步长时，仿真会迅速发散：
SIM_DURATION = 10 # (seconds) TIMESTEPS = np.power(10, np.linspace(-2, -1.5, 7)) # get plotting axes ax = plt.gca() for dt in TIMESTEPS: # set timestep model.opt.timestep = dt # allocate n_steps = int(SIM_DURATION / model.opt.timestep) sim_time = np.zeros(n_steps) energy = np.zeros(n_steps) * np.nan speed = np.zeros(n_steps) * np.nan # initialize mujoco.mj_resetData(model, data) data.qvel[0] = 11 # set root joint velocity # simulate print('simulating {} steps at dt = {:2.2g}ms'.format(n_steps, 1000*dt)) for i in range(n_steps): mujoco.mj_step(model, data) if data.warning.number.any(): warning_index = np.nonzero(data.warning.number)[0][0] warning = mujoco.mjtWarning(warning_index).name print(f'stopped due to divergence ({warning}) at timestep {i}.\n') break sim_time[i] = data.time energy[i] = sum(abs(data.qvel)) speed[i] = np.linalg.norm(data.qvel) # plot ax.plot(sim_time, energy, label='timestep = {:2.2g}ms'.format(1000*dt)) ax.set_yscale('log') # finalize plot ax.set_ybound(1, 1e3) ax.set_title('energy') ax.set_ylabel('Joule') ax.set_xlabel('second') ax.legend(frameon=True, loc='lower right'); plt.tight_layout()
3.2.5 接触
让我们回到 "方框和球体 "的示例，让它自由连接：
free_body_MJCF = """ <mujoco> <asset> <texture name="grid" type="2d" builtin="checker" rgb1=".1 .2 .3" rgb2=".2 .3 .4" width="300" height="300" mark="edge" markrgb=".2 .3 .4"/> <material name="grid" texture="grid" texrepeat="2 2" texuniform="true" reflectance=".2"/> </asset> <worldbody> <light pos="0 0 1" mode="trackcom"/> <geom name="ground" type="plane" pos="0 0 -.5" size="2 2 .1" material="grid" solimp=".99 .99 .01" solref=".001 1"/> <body name="box_and_sphere" pos="0 0 0"> <freejoint/> <geom name="red_box" type="box" size=".1 .1 .1" rgba="1 0 0 1" solimp=".99 .99 .01" solref=".001 1"/> <geom name="green_sphere" size=".06" pos=".1 .1 .1" rgba="0 1 0 1"/> <camera name="fixed" pos="0 -.6 .3" xyaxes="1 0 0 0 1 2"/> <camera name="track" pos="0 -.6 .3" xyaxes="1 0 0 0 1 2" mode="track"/> </body> </worldbody> </mujoco> """ model = mujoco.MjModel.from_xml_string(free_body_MJCF) renderer = mujoco.Renderer(model, 400, 600) data = mujoco.MjData(model) mujoco.mj_forward(model, data) renderer.update_scene(data, "fixed") media.show_image(renderer.render())
让这个体在地板上慢速滚动，同时想象接触点和力量：
n_frames = 200 height = 240 width = 320 frames = [] renderer = mujoco.Renderer(model, height, width) # visualize contact frames and forces, make body transparent options = mujoco.MjvOption() mujoco.mjv_defaultOption(options) options.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = True options.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = True options.flags[mujoco.mjtVisFlag.mjVIS_TRANSPARENT] = True # tweak scales of contact visualization elements model.vis.scale.contactwidth = 0.1 model.vis.scale.contactheight = 0.03 model.vis.scale.forcewidth = 0.05 model.vis.map.force = 0.3 # random initial rotational velocity: mujoco.mj_resetData(model, data) data.qvel[3:6] = 5*np.random.randn(3) # simulate and render for i in range(n_frames): while data.time < i/120.0: #1/4x real time mujoco.mj_step(model, data) renderer.update_scene(data, "track", options) frame = renderer.render() frames.append(frame) # show video media.show_video(frames, fps=30)
3.2.6 接触力分析
让我们重新运行上述仿真（使用不同的随机初始条件），并绘制一些与接触力相关的值
n_steps = 499 # allocate sim_time = np.zeros(n_steps) ncon = np.zeros(n_steps) force = np.zeros((n_steps,3)) velocity = np.zeros((n_steps, model.nv)) penetration = np.zeros(n_steps) acceleration = np.zeros((n_steps, model.nv)) forcetorque = np.zeros(6) # random initial rotational velocity: mujoco.mj_resetData(model, data) data.qvel[3:6] = 2*np.random.randn(3) # simulate and save data for i in range(n_steps): mujoco.mj_step(model, data) sim_time[i] = data.time ncon[i] = data.ncon velocity[i] = data.qvel[:] acceleration[i] = data.qacc[:] # iterate over active contacts, save force and distance for j,c in enumerate(data.contact): mujoco.mj_contactForce(model, data, j, forcetorque) force[i] += forcetorque[0:3] penetration[i] = min(penetration[i], c.dist) # we could also do # force[i] += data.qfrc_constraint[0:3] # do you see why? # plot _, ax = plt.subplots(3, 2, sharex=True, figsize=(10, 10)) lines = ax[0,0].plot(sim_time, force) ax[0,0].set_title('contact force') ax[0,0].set_ylabel('Newton') ax[0,0].legend(iter(lines), ('normal z', 'friction x', 'friction y')); ax[1,0].plot(sim_time, acceleration) ax[1,0].set_title('acceleration') ax[1,0].set_ylabel('(meter,radian)/s/s') ax[2,0].plot(sim_time, velocity) ax[2,0].set_title('velocity') ax[2,0].set_ylabel('(meter,radian)/s') ax[2,0].set_xlabel('second') ax[0,1].plot(sim_time, ncon) ax[0,1].set_title('number of contacts') ax[0,1].set_yticks(range(6)) ax[1,1].plot(sim_time, force[:,0]) ax[1,1].set_yscale('log') ax[1,1].set_title('normal (z) force - log scale') ax[1,1].set_ylabel('Newton') z_gravity = -model.opt.gravity[2] mg = model.body("box_and_sphere").mass[0] * z_gravity mg_line = ax[1,1].plot(sim_time, np.ones(n_steps)*mg, label='m*g', linewidth=1) ax[1,1].legend() ax[2,1].plot(sim_time, 1000*penetration) ax[2,1].set_title('penetration depth') ax[2,1].set_ylabel('millimeter') ax[2,1].set_xlabel('second') plt.tight_layout()
3.2.7 摩擦力
让我们看看改变摩擦力值的效果
MJCF = """ <mujoco> <asset> <texture name="grid" type="2d" builtin="checker" rgb1=".1 .2 .3" rgb2=".2 .3 .4" width="300" height="300" mark="none"/> <material name="grid" texture="grid" texrepeat="6 6" texuniform="true" reflectance=".2"/> <material name="wall" rgba='.5 .5 .5 1'/> </asset> <default> <geom type="box" size=".05 .05 .05" /> <joint type="free"/> </default> <worldbody> <light name="light" pos="-.2 0 1"/> <geom name="ground" type="plane" size=".5 .5 10" material="grid" zaxis="-.3 0 1" friction=".1"/> <camera name="y" pos="-.1 -.6 .3" xyaxes="1 0 0 0 1 2"/> <body pos="0 0 .1"> <joint/> <geom/> </body> <body pos="0 .2 .1"> <joint/> <geom friction=".33"/> </body> </worldbody> </mujoco> """ n_frames = 60 height = 300 width = 300 frames = [] # load model = mujoco.MjModel.from_xml_string(MJCF) data = mujoco.MjData(model) renderer = mujoco.Renderer(model, height, width) # simulate and render mujoco.mj_resetData(model, data) for i in range(n_frames): while data.time < i/30.0: mujoco.mj_step(model, data) renderer.update_scene(data, "y") frame = renderer.render() frames.append(frame) media.show_video(frames, fps=30)
四、肌腱、致动器和传感器
MJCF = """ """ model = mujoco.MjModel.from_xml_string(MJCF) renderer = mujoco.Renderer(model, 480, 480) data = mujoco.MjData(model) mujoco.mj_forward(model, data) renderer.update_scene(data, "fixed") media.show_image(renderer.render())
驱动蝙蝠和被动 "皮纳塔"：
n_frames = 180 height = 240 width = 320 frames = [] fps = 60.0 times = [] sensordata = [] renderer = mujoco.Renderer(model, height, width) # constant actuator signal mujoco.mj_resetData(model, data) data.ctrl = 20 # simulate and render for i in range(n_frames): while data.time < i/fps: mujoco.mj_step(model, data) times.append(data.time) sensordata.append(data.sensor('accelerometer').data.copy()) renderer.update_scene(data, "fixed") frame = renderer.render() frames.append(frame) media.show_video(frames, fps=fps)
让我们绘制加速度传感器测得的数值：
ax = plt.gca() ax.plot(np.asarray(times), np.asarray(sensordata), label='timestep = {:2.2g}ms'.format(1000*dt)) # finalize plot ax.set_title('Accelerometer values') ax.set_ylabel('meter/second^2') ax.set_xlabel('second') ax.legend(frameon=True, loc='lower right'); plt.tight_layout()
请注意，从加速度计的测量结果中可以清楚地看到身体被球棒击中的瞬间。
五、高级渲染
与关节可视化一样，其他渲染选项也作为参数显示在渲染方法中。
让我们回到第一个模型：
xml = """ """ model = mujoco.MjModel.from_xml_string(xml) renderer = mujoco.Renderer(model) data = mujoco.MjData(model) mujoco.mj_forward(model, data) renderer.update_scene(data) media.show_image(renderer.render())
#@title Enable transparency and frame visualization {vertical-output: true} scene_option.frame = mujoco.mjtFrame.mjFRAME_GEOM scene_option.flags[mujoco.mjtVisFlag.mjVIS_TRANSPARENT] = True renderer.update_scene(data, scene_option=scene_option) frame = renderer.render() media.show_image(frame)
#@title Depth rendering {vertical-output: true} # update renderer to render depth renderer.enable_depth_rendering() # reset the scene renderer.update_scene(data) # depth is a float array, in meters. depth = renderer.render() # Shift nearest values to the origin. depth -= depth.min() # Scale by 2 mean distances of near rays. depth /= 2*depth[depth <= 1].mean() # Scale to [0, 255] pixels = 255*np.clip(depth, 0, 1) media.show_image(pixels.astype(np.uint8)) renderer.disable_depth_rendering()
#@title Segmentation rendering {vertical-output: true} # update renderer to render segmentation renderer.enable_segmentation_rendering() # reset the scene renderer.update_scene(data) seg = renderer.render() # Display the contents of the first channel, which contains object # IDs. The second channel, seg[:, :, 1], contains object types. geom_ids = seg[:, :, 0] # Infinity is mapped to -1 geom_ids = geom_ids.astype(np.float64) + 1 # Scale to [0, 1] geom_ids = geom_ids / geom_ids.max() pixels = 255*geom_ids media.show_image(pixels.astype(np.uint8)) renderer.disable_segmentation_rendering()
5.1 摄像机矩阵
有关摄像机矩阵的描述，请参阅维基百科上的 "摄像机矩阵 "一文。
def compute_camera_matrix(renderer, data): """Returns the 3x4 camera matrix.""" # If the camera is a 'free' camera, we get its position and orientation # from the scene data structure. It is a stereo camera, so we average over # the left and right channels. Note: we call `self.update()` in order to # ensure that the contents of `scene.camera` are correct. renderer.update_scene(data) pos = np.mean([camera.pos for camera in renderer.scene.camera], axis=0) z = -np.mean([camera.forward for camera in renderer.scene.camera], axis=0) y = np.mean([camera.up for camera in renderer.scene.camera], axis=0) rot = np.vstack((np.cross(y, z), y, z)) fov = model.vis.global_.fovy # Translation matrix (4x4). translation = np.eye(4) translation[0:3, 3] = -pos # Rotation matrix (4x4). rotation = np.eye(4) rotation[0:3, 0:3] = rot # Focal transformation matrix (3x4). focal_scaling = (1./np.tan(np.deg2rad(fov)/2)) * renderer.height / 2.0 focal = np.diag([-focal_scaling, focal_scaling, 1.0, 0])[0:3, :] # Image matrix (3x3). image = np.eye(3) image[0, 2] = (renderer.width - 1) / 2.0 image[1, 2] = (renderer.height - 1) / 2.0 return image @ focal @ rotation @ translation
#@title Project from world to camera coordinates {vertical-output: true} # reset the scene renderer.update_scene(data) # Get the world coordinates of the box corners box_pos = data.geom_xpos[model.geom('red_box').id] box_mat = data.geom_xmat[model.geom('red_box').id].reshape(3, 3) box_size = model.geom_size[model.geom('red_box').id] offsets = np.array([-1, 1]) * box_size[:, None] xyz_local = np.stack(list(itertools.product(*offsets))).T xyz_global = box_pos[:, None] + box_mat @ xyz_local # Camera matrices multiply homogenous [x, y, z, 1] vectors. corners_homogeneous = np.ones((4, xyz_global.shape[1]), dtype=float) corners_homogeneous[:3, :] = xyz_global # Get the camera matrix. m = compute_camera_matrix(renderer, data) # Project world coordinates into pixel space. See: # https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula xs, ys, s = m @ corners_homogeneous # x and y are in the pixel coordinate system. x = xs / s y = ys / s # Render the camera view and overlay the projected corner coordinates. pixels = renderer.render() fig, ax = plt.subplots(1, 1) ax.imshow(pixels) ax.plot(x, y, '+', c='w') ax.set_axis_off()
5.2 修改场景
让我们在 mjvScene 中添加一些任意几何体。
def get_geom_speed(model, data, geom_name): """Returns the speed of a geom.""" geom_vel = np.zeros(6) geom_type = mujoco.mjtObj.mjOBJ_GEOM geom_id = data.geom(geom_name).id mujoco.mj_objectVelocity(model, data, geom_type, geom_id, geom_vel, 0) return np.linalg.norm(geom_vel) def add_visual_capsule(scene, point1, point2, radius, rgba): """Adds one capsule to an mjvScene.""" if scene.ngeom >= scene.maxgeom: return scene.ngeom += 1 # increment ngeom # initialise a new capsule, add it to the scene using mjv_makeConnector mujoco.mjv_initGeom(scene.geoms[scene.ngeom-1], mujoco.mjtGeom.mjGEOM_CAPSULE, np.zeros(3), np.zeros(3), np.zeros(9), rgba.astype(np.float32)) mujoco.mjv_makeConnector(scene.geoms[scene.ngeom-1], mujoco.mjtGeom.mjGEOM_CAPSULE, radius, point1[0], point1[1], point1[2], point2[0], point2[1], point2[2]) # traces of time, position and speed times = [] positions = [] speeds = [] offset = model.jnt_axis[0]/16 # offset along the joint axis def modify_scene(scn): """Draw position trace, speed modifies width and colors.""" if len(positions) > 1: for i in range(len(positions)-1): rgba=np.array((np.clip(speeds[i]/10, 0, 1), np.clip(1-speeds[i]/10, 0, 1), .5, 1.)) radius=.003*(1+speeds[i]) point1 = positions[i] + offset*times[i] point2 = positions[i+1] + offset*times[i+1] add_visual_capsule(scn, point1, point2, radius, rgba) duration = 6 # (seconds) framerate = 30 # (Hz) # Simulate and display video. frames = [] # Reset state and time. mujoco.mj_resetData(model, data) mujoco.mj_forward(model, data) while data.time < duration: # append data to the traces positions.append(data.geom_xpos[data.geom("green_sphere").id].copy()) times.append(data.time) speeds.append(get_geom_speed(model, data, "green_sphere")) mujoco.mj_step(model, data) if len(frames) < data.time * framerate: renderer.update_scene(data) modify_scene(renderer.scene) pixels = renderer.render() frames.append(pixels) media.show_video(frames, fps=framerate)
5.3 摄像机控制
摄像机可以动态控制，以实现电影效果。运行下面的三个单元格，就能看到静态摄像机和移动摄像机渲染效果的不同。
摄像机控制代码在两个轨迹之间平滑转换，一个轨迹围绕一个固定点运行，另一个轨迹跟踪一个移动物体。代码中的参数值是通过快速迭代低分辨率视频获得的。
#@title Load the "dominos" model dominos_xml = """ """ model = mujoco.MjModel.from_xml_string(dominos_xml) data = mujoco.MjData(model) renderer = mujoco.Renderer(model, height=1024, width=1440)
#@title Render from fixed camera duration = 2.5 # (seconds) framerate = 60 # (Hz) # Simulate and display video. frames = [] mujoco.mj_resetData(model, data) # Reset state and time. while data.time < duration: mujoco.mj_step(model, data) if len(frames) < data.time * framerate: renderer.update_scene(data, camera='top') pixels = renderer.render() frames.append(pixels) media.show_video(frames, fps=framerate)
#@title Render from moving camera duration = 3 # (seconds) # find time when box is thrown (speed > 2cm/s) throw_time = 0.0 mujoco.mj_resetData(model, data) while data.time < duration and not throw_time: mujoco.mj_step(model, data) box_speed = np.linalg.norm(data.joint('box').qvel[:3]) if box_speed > 0.02: throw_time = data.time assert throw_time > 0 def mix(time, t0=0.0, width=1.0): """Sigmoidal mixing function.""" t = (time - t0) / width s = 1 / (1 + np.exp(-t)) return 1 - s, s def unit_cos(t): """Unit cosine sigmoid from (0,0) to (1,1).""" return 0.5 - np.cos(np.pi*np.clip(t, 0, 1))/2 def orbit_motion(t): """Return orbit trajectory.""" distance = 0.9 azimuth = 140 + 100 * unit_cos(t) elevation = -30 lookat = data.geom('floor').xpos.copy() return distance, azimuth, elevation, lookat def track_motion(): """Return box-track trajectory.""" distance = 0.08 azimuth = 280 elevation = -10 lookat = data.geom('box').xpos.copy() return distance, azimuth, elevation, lookat def cam_motion(): """Return sigmoidally-mixed {orbit, box-track} trajectory.""" d0, a0, e0, l0 = orbit_motion(data.time / throw_time) d1, a1, e1, l1 = track_motion() mix_time = 0.3 w0, w1 = mix(data.time, throw_time, mix_time) return w0*d0+w1*d1, w0*a0+w1*a1, w0*e0+w1*e1, w0*l0+w1*l1 # Make a camera. cam = mujoco.MjvCamera() mujoco.mjv_defaultCamera(cam) # Simulate and display video. framerate = 60 # (Hz) slowdown = 4 # 4x slow-down mujoco.mj_resetData(model, data) frames = [] while data.time < duration: mujoco.mj_step(model, data) if len(frames) < data.time * framerate * slowdown: cam.distance, cam.azimuth, cam.elevation, cam.lookat = cam_motion() renderer.update_scene(data, cam) pixels = renderer.render() frames.append(pixels) media.show_video(frames, fps=framerate)
