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)