## Introduction

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.

## Steps

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]):
ab.append((a,b))

print(ab)

[([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)
print(filename)

with pygmsh.geo.Geometry() as geom:
#create a triangle from 0,0 to the next two vertices
#extrude the polygon
geom.extrude(poly, [0.0, 0, thickness], num_layers=1)
mesh = geom.generate_mesh()
filenames.append(filename)
mesh.write(filename+'.stl',file_format='stl',binary=True)


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 = """
<mujoco>
<option>
<flag contact="disable" />
</option>
<asset>{filenames}
</asset>
<worldbody>
<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"/>
<body>
<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"/>
<body>
<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"/>
<body>
<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"/>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<equality>
<weld name="weld1" active="true" body1="A" body2="Ap" relpose="0 0 0 1 0 0 0" anchor="0 0 0" />
</equality>
</mujoco>
"""


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)
filename_string+=s

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])

xml=xml_template.format(j1=v1s,j2=v2s,j3=v3s,j4=v4s,filenames=filename_string)


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:
renderer.update_scene(data)
pixels = renderer.render()
frames.append(pixels)

media.show_video(frames, fps=framerate)

 This browser does not support the video tag.