def reset_recording(self):
"""
Resets all recorded data.
"""
# Note: This syntax was chosen to match PyPlotVisualizer.
self._animation = Animation(default_framerate=1. / self.draw_period)
self._recording_frame_num = 0
# PD controller on base angle
Kp = 150
Kd = 0.01
def pd_controller(t, q, v, q_ref, v_ref, a_ref):
pitch = se3.rpy.matrixToRpy(se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix())[1, 0]
dpitch = v[4, 0]
u = - np.matrix([[Kp * (pitch + Kd * dpitch)]])
# -u: torque is applied to the wheel, so the opposite torque is applied to the base.
return -u
# Create the viewer.
viewer = meshcat.visualizer.Visualizer("tcp://127.0.0.1:6000")
# Display the simulation as a pure animation after the simulation, so the display can be to be real time.
anim = Animation(default_framerate = 1 / (refresh_rate * dt))
# Initialize claptrap object
claptraps = {}
claptraps["ZeroTorque"] = Claptrap(zero_torque_controller, meshcat_viewer=viewer, meshcat_name = "ZeroTorque", robot_color = np.array([1.0, 0.0, 0.0, 0.5]))
claptraps["PD"] = Claptrap(pd_controller, meshcat_viewer=viewer, meshcat_name = "PD", robot_color = np.array([0.0, 1.0, 0.0, 1.0]))
# Set initial state
quat = se3.Quaternion(se3.rpy.rpyToMatrix(np.matrix([[0.0, 0.2, 0.0]]).T))
for c in claptraps:
claptraps[c].q[3:7, 0] = quat.coeffs()
claptraps[c].q[:3, 0] = claptraps[c].wheel_radius
claptraps[c].solver.set_initial_value(np.concatenate((claptraps[c].q, claptraps[c].v)))
# Initialize logger
logged_values = []
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)
