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