import mujoco, numpy as np
import xml.etree.ElementTree as ET
from PIL import Image
root=ET.parse("gripper_testing.urdf").getroot()
for l in root.findall("link"):
    if l.find("inertial") is None:
        i=ET.SubElement(l,"inertial"); ET.SubElement(i,"mass",{"value":"0.1"})
        ET.SubElement(i,"inertia",dict(ixx="1e-4",ixy="0",ixz="0",iyy="1e-4",iyz="0",izz="1e-4"))
mj=ET.Element("mujoco"); c=ET.SubElement(mj,"compiler"); c.set("discardvisual","false"); c.set("meshdir","."); v=ET.SubElement(mj,"visual"); ET.SubElement(v,"global",{"offwidth":"1600","offheight":"1000"}); root.insert(0,mj)
m=mujoco.MjModel.from_xml_string(ET.tostring(root,encoding="unicode")); d=mujoco.MjData(m); mujoco.mj_forward(m,d)
ims=[]
for az,el,dist in [(90,-88,0.09),(35,-35,0.11)]:
    cam=mujoco.MjvCamera(); cam.lookat[:]= [-0.0255,0.0,0.014]; cam.distance=dist; cam.azimuth=az; cam.elevation=el
    with mujoco.Renderer(m,470,630) as r: r.update_scene(d,cam); ims.append(r.render())
Image.fromarray(np.hstack(ims)).save("plate_zoom.png"); print("wrote plate_zoom.png")
