def visualize(self, states, dt): import meshcat import meshcat.geometry as g import meshcat.transformations as tf import time # Create a new visualizer vis = meshcat.Visualizer() vis.open() vis["/Cameras/default"].set_transform( tf.translation_matrix([0, 0, 0]).dot( tf.euler_matrix(0, np.radians(-30), -np.pi / 2))) vis["/Cameras/default/rotated/<object>"].set_transform( tf.translation_matrix([1, 0, 0])) vis["Quadrotor"].set_object( g.StlMeshGeometry.from_file('systems/crazyflie2.stl')) while True: for state in states: vis["Quadrotor"].set_transform( tf.translation_matrix([state[0], state[1], state[2]]).dot( tf.quaternion_matrix(state[6:10]))) time.sleep(dt)
def setCameraTransform(self, translation=np.zeros(3), rotation=np.zeros(3), relative=False): # translation : [Px, Py, Pz], # rotation : [Roll, Pitch, Yaw] R_pnc = rpyToMatrix(np.array(rotation)) if Viewer.backend == 'gepetto-gui': H_abs = SE3(R_pnc, np.array(translation)) if relative: H_orig = XYZQUATToSe3( self._client.getCameraTransform(self._window_id)) H_abs = H_abs * H_orig self._client.setCameraTransform(self._window_id, se3ToXYZQUAT(H_abs).tolist()) else: if relative: raise RuntimeError( "'relative'=True not available with meshcat.") import meshcat.transformations as tf # Transformation of the camera object T_meshcat = tf.translation_matrix(translation) self._client.viewer[ "/Cameras/default/rotated/<object>"].set_transform(T_meshcat) # Orientation of the camera object Q_pnc = Quaternion(R_pnc).coeffs() Q_meshcat = np.roll(Q_pnc, shift=1) R_meshcat = tf.quaternion_matrix(Q_meshcat) self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def visualize_train_input(self, in_img, p, q, theta, z, roll, pitch): """Visualize the training input.""" points = [] colors = [] height = in_img[:, :, 3] for i in range(in_img.shape[0]): for j in range(in_img.shape[1]): pixel = (i, j) position = utils.pixel_to_position(pixel, height, self.bounds, self.pixel_size) points.append(position) colors.append(in_img[i, j, :3]) points = np.array(points).T # shape (3, N) colors = np.array(colors).T / 255.0 # shape (3, N) self.vis["pointclouds/scene"].set_object( g.PointCloud(position=points, color=colors)) pick_position = utils.pixel_to_position(p, height, self.bounds, self.pixel_size) label = "pick" utils.make_frame(self.vis, label, h=0.05, radius=0.0012, o=0.1) pick_transform = np.eye(4) pick_transform[0:3, 3] = pick_position self.vis[label].set_transform(pick_transform) place_position = utils.pixel_to_position(q, height, self.bounds, self.pixel_size) label = "place" utils.make_frame(self.vis, label, h=0.05, radius=0.0012, o=0.1) place_transform = np.eye(4) place_transform[0:3, 3] = place_position place_transform[2, 3] = z rotation = utils.get_pybullet_quaternion_from_rot( (roll, pitch, -theta)) quaternion_wxyz = np.asarray( [rotation[3], rotation[0], rotation[1], rotation[2]]) place_transform[0:3, 0:3] = mtf.quaternion_matrix(quaternion_wxyz)[0:3, 0:3] self.vis[label].set_transform(place_transform) _, ax = plt.subplots(2, 1) ax[0].imshow(in_img.transpose(1, 0, 2)[:, :, :3] / 255.0) ax[0].scatter(p[0], p[1]) ax[0].scatter(q[0], q[1]) ax[1].imshow(in_img.transpose(1, 0, 2)[:, :, 3]) ax[1].scatter(p[0], p[1]) ax[1].scatter(q[0], q[1]) plt.show()
def meshcat_visualize(vis, obs, act, info): """Visualize data using meshcat.""" for key in sorted(info.keys()): pose = info[key] pick_transform = np.eye(4) pick_transform[0:3, 3] = pose[0] quaternion_wxyz = np.asarray( [pose[1][3], pose[1][0], pose[1][1], pose[1][2]]) pick_transform[0:3, 0:3] = mtf.quaternion_matrix(quaternion_wxyz)[0:3, 0:3] label = 'obj_' + str(key) make_frame(vis, label, h=0.05, radius=0.0012, o=1.0) vis[label].set_transform(pick_transform) for cam_index in range(len(act['camera_config'])): verts = unproject_depth_vectorized( obs['depth'][cam_index], [0, 1], np.array(act['camera_config'][cam_index]['intrinsics']).reshape( 3, 3), np.zeros(5)) # switch from [N,3] to [3,N] verts = verts.T cam_transform = np.eye(4) cam_transform[0:3, 3] = act['camera_config'][cam_index]['position'] quaternion_xyzw = act['camera_config'][cam_index]['rotation'] quaternion_wxyz = np.asarray([ quaternion_xyzw[3], quaternion_xyzw[0], quaternion_xyzw[1], quaternion_xyzw[2] ]) cam_transform[0:3, 0:3] = mtf.quaternion_matrix(quaternion_wxyz)[0:3, 0:3] verts = apply_transform(cam_transform, verts) colors = obs['color'][cam_index].reshape(-1, 3).T / 255.0 vis['pointclouds/' + str(cam_index)].set_object( g.PointCloud(position=verts, color=colors))
def setCameraTransform(self, translation, rotation): # translation : [Px, Py, Pz], # rotation : [Roll, Pitch, Yaw] R_pnc = rpyToMatrix(np.array(rotation)) if Viewer.backend == 'gepetto-gui': T_pnc = np.array(translation) T_R = SE3(R_pnc, T_pnc) self._client.setCameraTransform(self._window_id, se3ToXYZQUAT(T_R).tolist()) else: import meshcat.transformations as tf # Transformation of the camera object T_meshcat = tf.translation_matrix(translation) self._client.viewer[ "/Cameras/default/rotated/<object>"].set_transform(T_meshcat) # Orientation of the camera object Q_pnc = Quaternion(R_pnc).coeffs() Q_meshcat = np.roll(Q_pnc, shift=1) R_meshcat = tf.quaternion_matrix(Q_meshcat) self._client.viewer["/Cameras/default"].set_transform(R_meshcat)