Simulating Spherical Mechanisms in MuJoCo


    The purpose of this example is to show how easy it is to create spherical mechanisms in MuJoCo.

    MuJoCo is a great resource for studying dynamics of rigid body systems, is great for robotics, and has been a recent focus of mine as I am converting some of my course material over to use it for studying foldable mechanisms.

    This example goes through the following steps. It permits you to specify four joints in the x-y plane that serve as your spherical joints. It then creates four thin meshes based on those joint locations and populates an xml template with the joint locations and mesh file information. The simulation then runs, showing off the parallel nature of the mechanism

    The <weld> element in the xml is responsible for closing the kinematic loop, and is quite easy to use for flat-foldable kinematics. One thing to note in my example is that I create a duplicate body ("Ap") to my fixed base ("A"), and then use a weld to constrain ("A") to ("Ap"). Because the mechanism is defined in its flat state, the transformation to map ("Ap") to ("A") needs no offset and no rotation (relpose="0 0 0 1 0 0 0" anchor="0 0 0"). Below is a description of the code in more detail.


    First, import the necessary packages

    import os
    import mujoco
    import numpy
    import mediapy as media
    import matplotlib.pyplot as plt
    import pygmsh

    Define four joints as 3d vectors in the xy plane

    v1 = [0,-1,0]
    v2 = [1,-1,0]
    v3 = [1,1,0]
    v4 = [-2,1,0]

    Assemble vectors into sequences I can use for generating meshes

    v = v1,v2,v3,v4
    ab = []
    for a,b in zip(v,v[1:]+v[:-1]):
    [([0, -1, 0], [1, -1, 0]), ([1, -1, 0], [1, 1, 0]), ([1, 1, 0], [-2, 1, 0]), ([-2, 1, 0], [0, -1, 0])]

    Create meshes by extruding triangles by a thickness

    thickness = .1
    filenames = []
    for ii,(a,b) in enumerate(ab):
        filename = 'mesh_{0:02.0f}'.format(ii)
        with pygmsh.geo.Geometry() as geom:
            #create a triangle from 0,0 to the next two vertices
            poly = geom.add_polygon([(0,0),a[:2],b[:2]],mesh_size=2)
            #extrude the polygon
            geom.extrude(poly, [0.0, 0, thickness], num_layers=1)
            mesh = geom.generate_mesh()

    I got some warnings about meshio here, which can be ignored.

    It should be noted that pygmsh does not do a good job of finding the proper vertex order for identifying outward normal vectors for faces, which could lead to improper mass calculations if not handled properly by mujoco. Future work should include finding a better mesher, or, temporarily, using meshlab to correct triangle normals to face outward, as is standard.

    Define xml template. The filenames and joint axes are automatically generated.

    (This should eventually be further abstracted so as to generate n bodies (recursively), but for now its statically defined for four bodies.)

    xml_template = """
         <flag contact="disable" />
        <light name="top" pos="0 0 1"/>
        <body  name="A">
          <geom type="mesh" pos="0 0 0"  density="1000" mesh="mesh_00" rgba="1 0 0 1"/>
              <joint type="hinge" axis="{j2}" pos="0 0 0"/>
              <geom type="mesh" pos="0 0 0"  density="1000" mesh="mesh_01" rgba="1 0 1 1"/>
                  <joint type="hinge" axis="{j3}" pos="0 0 0"/>
                  <geom type="mesh" pos="0 0 0"  density="1000" mesh="mesh_02" rgba="1 1 0 1"/>
                      <joint type="hinge" axis="{j4}" pos="0 0 0"/>
                      <geom type="mesh" pos="0 0 0"  density="1000" mesh="mesh_03" rgba="0 1 0 1"/>
                    <body name="Ap">
                      <joint type="hinge" axis="{j1}" pos="0 0 0"/>
                      <geom type="mesh" pos="0 0 0"  density="1000" mesh="mesh_00" rgba="1 0 0 1"/>
          <weld name="weld1" active="true" body1="A" body2="Ap" relpose="0 0 0 1 0 0 0" anchor="0 0 0" />

    populate xml template with joint and file information

    filename_string = ''
    for filename in filenames:
        s='\n<mesh name="{filename}" file="{filename}.stl" />'.format(filename=filename)
    v1s = ' '.join([str(item) for item in v1])
    v2s = ' '.join([str(item) for item in v2])
    v3s = ' '.join([str(item) for item in v3])
    v4s = ' '.join([str(item) for item in v4])

    print(xml) Run Simulation

    model = mujoco.MjModel.from_xml_string(xml)
    data = mujoco.MjData(model)
    renderer = mujoco.Renderer(model)
    duration = 10
    framerate = 30
    frames = []
    mujoco.mj_resetData(model, data)
    while data.time < duration:
        mujoco.mj_step(model, data)
        if len(frames) < data.time * framerate:
            pixels = renderer.render()
    media.show_video(frames, fps=framerate)