n_iter = 0
t = dt
success = True
while success and t < simulation_length:
success = True
n_iter = n_iter + 1
for c in claptraps:
success &= claptraps[c].integrate(t)
# Log
claptraps[c].log_state(logger, "ZeroTorque.")
# Update display
if n_iter % refresh_rate == 0:
with anim.at_frame(viewer, n_iter / refresh_rate) as frame:
claptraps[c].robot.viz.viewer = frame
claptraps[c].robot.display(claptraps[c].q)
logger.set("time", t)
t += dt
logger.new_line()
sys.stdout.write('Running simulation: {:0.1f}/{}s\r'.format(t, simulation_length))
sys.stdout.flush()
logger.save('/tmp/claptrap_simulation.csv')
# Display animation
viewer.set_animation(anim)
def animate(self, chain, states, framerate=5, showMeshes=False):
if 'google.colab' in sys.modules:
server_args = ['--ngrok_http_tunnel']
# Start a single meshcat server instance to use for the remainder of this notebook.
from meshcat.servers.zmqserver import start_zmq_server_as_subprocess
proc, zmq_url, web_url = start_zmq_server_as_subprocess(
server_args=server_args)
vis = meshcat.Visualizer(zmq_url=zmq_url)
else:
vis = meshcat.Visualizer().open()
anim = Animation()
vertices = chain.get_vertex_coords()
if showMeshes:
for i, link in enumerate(chain.linkArray):
if link.meshObj == None:
print("No mesh: " + link.name)
continue
boxVis = vis["link:" + link.name]
boxVis.set_object(
link.meshObj,
g.MeshLambertMaterial(color=0xffffff, reflectivity=0.8))
rotationMatrix = np.pad(link.absoluteOrientation, [(0, 1),
(0, 1)],
mode='constant')
rotationMatrix[-1][-1] = 1
boxVis.set_transform(
tf.translation_matrix(link.absoluteBase) @ rotationMatrix)
for i in range(len(states)):
chain.update(states[i])
with anim.at_frame(vis, framerate * i) as frame:
for i, link in enumerate(chain.linkArray):
if link.meshObj == None:
continue
boxVis = frame["link:" + link.name]
rotationMatrix = np.pad(link.absoluteOrientation,
[(0, 1), (0, 1)],
mode='constant')
rotationMatrix[-1][-1] = 1
boxVis.set_transform(
tf.translation_matrix(link.absoluteBase)
@ rotationMatrix)
else:
for i in range(int(vertices.shape[0] / 2)):
p1 = vertices[2 * i]
p2 = vertices[2 * i + 1]
cylinder_transform = getCylinderTransform(p1, p2)
boxVis = vis["link" + str(i)]
boxVis.set_object(g.Cylinder(1, 0.01))
boxVis.set_transform(cylinder_transform)
for i in range(len(states)):
chain.update(states[i])
with anim.at_frame(vis, framerate * i) as frame:
vertices = chain.get_vertex_coords()
for i in range(int(vertices.shape[0] / 2)):
p1 = vertices[2 * i]
p2 = vertices[2 * i + 1]
cylinder_transform = getCylinderTransform(p1, p2)
boxVis = frame["link" + str(i)]
boxVis.set_transform(cylinder_transform)
vis.set_animation(anim)
