def __init__(self, center: Vector3r, width: Vector3r, height: Vector3r): """ Create a new rectangle representation in 3D. """ self.center = center self.half_width = width / 2.0 self.half_height = height / 2.0 self._S = Vector3r(1.0, 1.0, 1.0) # scaling self._T = Vector3r(0.0, 0.0, 0.0) # translation
def collect_points_6dir(self, tgt): # must be in CV mode, otherwise the point clouds won't align scan_config = [0, -1, 2, 1, 4, 5] # front, left, back, right, up, down points_6dir = np.zeros((0, self.imgwidth, 3), dtype=np.float32) for k, face in enumerate(scan_config): if face == 4: # look upwards at tgt pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]), to_quaternion(math.pi / 2, 0, 0)) # up elif face == 5: # look downwards pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]), to_quaternion(-math.pi / 2, 0, 0)) # down - pitch, roll, yaw else: # rotate from [-90, 0, 90, 180] yaw = math.pi / 2 * face pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]), to_quaternion(0, 0, yaw)) self.set_vehicle_pose(pose) depth_front, _ = self.get_depth_campos() if depth_front is None: rospy.logwarn('Missing image {}: {}'.format(k, tgt)) continue # import ipdb;ipdb.set_trace() point_array = expo_util.depth_to_point_cloud(depth_front, self.focal, self.pu, self.pv, mode=k) points_6dir = np.concatenate((points_6dir, point_array), axis=0) # reset the pose for fun pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]), to_quaternion(0, 0, 0)) self.set_vehicle_pose(pose) # print 'points:', points_6dir.shape return points_6dir
def _drop_till_obstacle_detected(self, speed=5.0): """Move downward vertically untill an obstacle is detected by lidar sensor.""" current_pos = self._uav.simGetGroundTruthKinematics().position self._uav.moveByVelocityAsync(0, 0, speed, duration=1000) while True: try: lidar_data = self._uav.getLidarData( lidar_name="BottomObstacleDetector") if len(lidar_data.point_cloud) >= 3: uav_pos = self._uav.simGetGroundTruthKinematics().position for i in range(0, len(lidar_data.point_cloud), 3): p = Vector3r( lidar_data.point_cloud[i], lidar_data.point_cloud[i + 1], lidar_data.point_cloud[i + 2], ) delta_height = p.z_val - uav_pos.z_val inside_circle = (Vector3r( p.x_val, p.y_val, 0).distance_to( Vector3r(uav_pos.x_val, uav_pos.y_val, 0)) < 2) if delta_height > 0 and inside_circle: self._uav.moveByVelocityAsync(0, 0, 0, duration=1.0) return p.z_val except AttributeError: print("AttributeError") continue
def frustum_plot_list_from_viewport_vectors( pose: Pose, eye_to_top_left: np.ndarray, eye_to_top_right: np.ndarray, eye_to_bottom_left: np.ndarray, eye_to_bottom_right: np.ndarray, scale: float = 2.0 ) -> List[Vector3r]: eye = pose.position top_left = eye + Vector3r(*(eye_to_top_left * scale)) top_right = eye + Vector3r(*(eye_to_top_right * scale)) bottom_left = eye + Vector3r(*(eye_to_bottom_left * scale)) bottom_right = eye + Vector3r(*(eye_to_bottom_right * scale)) return [ eye, top_left, eye, top_right, eye, bottom_left, eye, bottom_right, top_left, top_right, top_right, bottom_right, bottom_right, bottom_left, bottom_left, top_left, ]
def quaternion_from_two_vectors(a: Vector3r, b: Vector3r) -> Quaternionr: """ What rotation (around the intersection of the two vectors) we need to rotate `a` to `b`? """ # ref.: https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h (FromTwoVectors) v0 = a / a.get_length() v1 = b / b.get_length() c = v1.dot(v0) assert c > -1 # FIXME handle the case when the vectors are nearly opposites s = np.sqrt((1 + c) * 2) axis = v0.cross(v1) / s return Quaternionr(axis.x_val, axis.y_val, axis.z_val, w_val=(s / 2))
def fly(client: MultirotorClient, args: argparse.Namespace) -> None: reset(client) rect = cast(Rect, args.rect) initial_pose = cast(Pose, client.simGetVehiclePose()) closest_corner = rect.closest_corner(initial_pose.position) if args.verbose: ff.print_pose(initial_pose, airsim.to_eularian_angles) ff.log_info(f"Closest corner {ff.to_xyz_str(closest_corner)}") start_pos = Vector3r( *ff.to_xyz_tuple(closest_corner)) # NOTE make sure we copy it by value start_pos.z_val += args.z_offset start_pos.y_val += args.y_offset start_pos.x_val += args.x_offset plot = Plotter(client) plot.flush_persistent() plot.points([start_pos], Rgba.Red) if args.teleport: ff.log(f"Teleporting to {ff.to_xyz_str(start_pos)}...") new_pose = Pose(start_pos, initial_pose.orientation) Controller.teleport(client, to=new_pose, ignore_collision=True) else: ff.log(f"Flying to {ff.to_xyz_str(start_pos)}...") client.moveToPositionAsync(*ff.to_xyz_tuple(start_pos), velocity=5).join() ff.log("Capturing grid...") time.sleep(2) z = Vector3r(0, 0, args.altitude_shift) path = [ z + Vector3r(*ff.to_xyz_tuple(corner)) for corner in rect.zigzag(lanes=args.zigzags) ] if AUGMENT_PATHS: path = Controller.augment_path(path, AUGMENT_PATHS_MAX_DIST) plot.points(path, Rgba.White) plot.lines(path, Rgba.Green) try: if RECORD: client.startRecording() Controller.fly_path(client, path) except Exception as e: raise e finally: if RECORD: client.stopRecording()
def from_dump(dump_str: str) -> Rect: """ Convert `dump_str`, a string representation of a rectangle, back to a `Rect`. Note: `dump_str` is assumed to have been created by calling the `Rect.to_dump` method. """ import json json_repr = json.loads(dump_str) return Rect( Vector3r(*json_repr["center"]), Vector3r(*[2 * _ for _ in json_repr["half_width"]]), Vector3r(*[2 * _ for _ in json_repr["half_height"]]), )
def align_meshroom_to_airsim(meshroom_pose, meshroom_to_airsim_transform): # NOTE this transformation is only based on the positions (and not on the orientations) meshroom_pos = np.append(meshroom_pose.position.to_numpy_array(), 1) # [x, y, z, 1] airsim_pos = np.matmul(meshroom_to_airsim_transform, meshroom_pos) return Pose(Vector3r(*airsim_pos), meshroom_pose.orientation)
def convert_2_pose(p, e): """ p: A 3-element NumPy array, the position. q: A 4-element NumPy array, a quaternion witht he last element being w. """ return Pose(Vector3r(p[0], p[1], p[2]), to_quaternion(e[0], e[1], e[2]))
def convert_2_pose(a): """ a: A 7-element array. """ return Pose(Vector3r(a[0], a[1], a[2]), Quaternionr(a[3], a[4], a[5], a[6]))
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: # Reset the drone client.reset() client.enableApiControl(True) client.armDisarm(True) # Draw the ROI outline (erasing previous plots) client.simFlushPersistentMarkers() if SHOW_PLOTS: client.simPlotLineStrip(points=args.roi.corners(repeat_first=True), is_persistent=True) # Get the first position the drone will fly to initial_pose = client.simGetVehiclePose() closest_corner = args.roi.closest_corner(initial_pose.position) if args.verbose: ff.print_pose(initial_pose, airsim.to_eularian_angles) ff.log_info(f"Closest corner {ff.to_xyz_str(closest_corner)}") start_pos = Vector3r( *ff.to_xyz_tuple(closest_corner if args.corner else args.roi.center)) # NOTE AirSim uses NED coordinates, so negative Z values are "up" actually if (z := args.z_offset): start_pos.z_val -= z # start higher up, to avoid crashing with objects
def quaternion_from_rotation_axis_angle(axis: Vector3r, angle: float, is_degrees: bool = False) -> Quaternionr: if is_degrees: angle = np.deg2rad(angle) half_angle = angle / 2 axis /= axis.get_length() # normalize axis *= np.sin(half_angle) return Quaternionr(axis.x_val, axis.y_val, axis.z_val, w_val=np.cos(half_angle))
def data_sampling(self, positions, trajname): self.init_folders(trajname) self.randquaternionsampler.init_random_yaw() start = time.time() for k, pose in enumerate(positions): position = Vector3r(pose[0], pose[1], pose[2]) orientation = self.randquaternionsampler.next_quaternion() dronepose = Pose(position, orientation) self.imgclient.setpose(dronepose) time.sleep(0.02) self.imgclient.simPause(True) rgblist, depthlist, seglist, camposelist = self.imgclient.readimgs( ) self.imgclient.simPause(False) # save images and poses imgprefix = str(k).zfill(6) + '_' for w, camind in enumerate(self.camlist): camname = self.camlist_name[camind] # save RGB image if 'Scene' in self.imgtypelist: img = rgblist[w] # change bgr to rgb cv2.imwrite( join(self.imgdirs[w], imgprefix + camname + '.png'), img, [cv2.IMWRITE_PNG_COMPRESSION, 0]) # save depth image if 'DepthPlanner' in self.imgtypelist: depthimg = depthlist[w] np.save( join(self.depthdirs[w], imgprefix + camname + '_depth.npy'), depthimg) # save segmentation image if 'Segmentation' in self.imgtypelist: segimg = seglist[w] np.save( join(self.segdirs[w], imgprefix + camname + '_seg.npy'), segimg) # write pose to file self.posenplist[w].append(np.array(camposelist[w])) # imgshow = np.concatenate((leftimg,rightimg),axis=1) print( ' {0}, pose {1}, orientation ({2:.2f},{3:.2f},{4:.2f},{5:.2f})' .format(k, pose, orientation.x_val, orientation.y_val, orientation.z_val, orientation.w_val)) # cv2.imshow('img',imgshow) # cv2.waitKey(1) for w in range(len(self.camlist)): # save poses into numpy txt np.savetxt(self.posefilelist[w], self.posenplist[w]) end = time.time() print('Trajectory sample time {}'.format(end - start))
def fly_zone(client: airsim.MultirotorClient, zone: Rect, altitude_shift: float = 0.0) -> None: path = [ Vector3r(corner.x_val, corner.y_val, corner.z_val - altitude_shift) for corner in zone.corners(repeat_first=True) ] if AUGMENT_PATHS: path = Controller.augment_path(path, max_dist=2) if SHOW_PLOTS: client.simPlotPoints(points=path, is_persistent=True, color_rgba=Rgba.White, size=5) client.simPlotLineStrip(points=path, is_persistent=True, color_rgba=Rgba.White) # Controller.fly_path(client, path) ##client.moveOnPathAsync(path, velocity=2).join() # XXX testing.. stretching Rect, zigzagging path and flying over it zz_path = Rect( Vector3r(0, 0, -altitude_shift) + zone.center, # Vector3r(0, 0, -altitude_shift) + zone.half_width * 4, # Vector3r(0, 0, -altitude_shift) + zone.half_height * 4, zone.half_width * 2, zone.half_height * 2, ).zigzag(4) if AUGMENT_PATHS: zz_path = Controller.augment_path(zz_path, max_dist=2) if SHOW_PLOTS: client.simPlotPoints(points=zz_path, is_persistent=True, color_rgba=Rgba.White, size=5) client.simPlotLineStrip(points=zz_path, is_persistent=True, color_rgba=Rgba.Green) Controller.fly_path( client, zz_path) ##client.moveOnPathAsync(zz_path, velocity=2).join()
def _move_to(self, target, speed=5.0, yaw=0.0): """Due to bugs in controlling the drone(issue 1677 and 1292), the temporal solution is presented.""" target = (Vector3r(target[0], target[1], target[2]) if type(target) == list else target) self._uav.moveToPositionAsync( target.x_val, target.y_val, target.z_val, speed, yaw_mode=airsim.YawMode(False, yaw), ).join()
def explore(self): """An offline method of exploration.""" img_dir = self._config["image_dir"] # Generate flight plan views = self.generate_explore_views() # Arm and takeoff self._uav.armDisarm(True) self._uav.takeoffAsync().join() current_pos = self._uav.simGetGroundTruthKinematics().position self._move_to(Vector3r(current_pos.x_val, current_pos.y_val, -60)) # Fly to each view and take photos for idx, view in enumerate(views): self._observe_at_view(view, os.path.join(img_dir, "%s.png" % idx))
def init_vehicle(self): # Set Drive Commands to zero initially self._throttle = 0 # Used as a constant gain factor for the action throttle. self._steering = 0 # Each action lasts this many seconds self._brake = 0 self.THROTTLE_INC = .10 self.THROTTLE_DEC = -.10 self.BRAKE_INC = .10 self.BRAKE_DEC = -.20 self.STEER_LEFT_INC = -.10 self.STEER_RIGHT_INC = .10 # Connect to the AirSim simulator and begin: print('Initializing Car Client') self.client = client.CarClient() self.client.confirmConnection() self.client.enableApiControl(True) print('Initialization DONE!') print("Setting Camera Views") orien = Vector3r(0, 0, 0) self.client.simSetCameraOrientation(0, orien) #radians orien = Vector3r(0, .12, -np.pi/9) self.client.simSetCameraOrientation(1, orien) orien = Vector3r(0, .12, np.pi/9) self.client.simSetCameraOrientation(2, orien) # Reset Collion Flags print("Setting Camera Views DONE!") # Set up GUI Video Feeder num_video_feeds = np.sum(np.array(self.image_mask_FC_FR_FL, dtype = np.int)) * self.IMG_STEP GUIConn, self.simEnvDataConn = multiprocessing.Pipe() self.app = AirSimGUI.CarGUI(GUIConn, vehicle_names = [self.vehicle_name], num_video_feeds = num_video_feeds, isRGB = self.isRGB, isNormal = self.isNormal)
def vector_rotated_by_quaternion(v: Vector3r, q: Quaternionr) -> Vector3r: q /= q.get_length() # normalize # Extract the vector and scalar parts of q: u, s = Vector3r(q.x_val, q.y_val, q.z_val), q.w_val # NOTE the results from these two methods are the same up to 7 decimal places. # ref.: https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion # return u * (2 * u.dot(v)) + v * (s * s - u.dot(u)) + u.cross(v) * (2 * s) # ref.: https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h (_transformVector) uv = u.cross(v) uv += uv return v + uv * s + u.cross(uv)
def __init__( self, time_stamp: int, pos_x: float, pos_y: float, pos_z: float, q_w: float, q_x: float, q_y: float, q_z: float, image_file: str, ): """ Represents an item from AirSim's recording file (i.e. a row in `airsim_rec.txt`). Reference: https://github.com/microsoft/AirSim/blob/master/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp """ # assert os.path.isfile(image_file), image_file self.time_stamp = time_stamp self.image_file = image_file self.position = Vector3r(pos_x, pos_y, pos_z) self.orientation = Quaternionr(q_x, q_y, q_z, q_w)
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: if args.flush: client.simFlushPersistentMarkers() if not args.transformation: poses = [ Pose(record.position, record.orientation) for record in args.recording.values() ] positions = [record.position for record in args.recording.values()] else: matrix = np.loadtxt( args.transformation) # load the 4x4 transformation matrix print(matrix) poses = [] positions = [] for record in args.recording.values(): pos = Vector3r(*np.matmul(matrix, np.append(record.position, 1))) poses.append(Pose(pos, record.orientation)) positions = [pos] if args.axes: client.simPlotTransforms(poses, scale=100.0, thickness=2.5, is_persistent=True) else: client.simPlotPoints(positions, args.color, size=10, is_persistent=True) if args.lines: client.simPlotLineStrip(positions, args.color, thickness=2.5, is_persistent=True) if args.aim: line_list = [] for pose in poses: line_list.append(pose.position) x_axis, _, _ = AirSimNedTransform.local_axes_frame(pose) line_list.append(pose.position + x_axis * 100) client.simPlotLineList(line_list, args.color, thickness=2.5, is_persistent=True)
def explore(self): if not self._safety_surface["type"] == "circle": return img_dir = self._config["image_dir"] # Arm and takeoff self._uav.armDisarm(True) self._uav.takeoffAsync().join() self._move_to(Vector3r(0, 0, -60)) # Explore center point and points on edge root = self.generate_explore_views() DETE.show_explore_path(root) center = root triangles = root.dfs() # all_points = [center.pc, center.childs[0].p2] print("Center point, counter=1") self.explore_point(center.pc, center, center.pc, os.path.join(img_dir, "1.png")) print("Edge point, counter=2") photo_counter = 2 self.explore_point( center.childs[0].p2, center.childs[0], center.pc, os.path.join(img_dir, "2.png"), ) # Iteration: dividing triangles for triangle in triangles: if triangle.parent.is_root: # all_points.extend([triangle.p3, triangle.pc]) photo_counter += 1 print("Edge point, counter=%d" % photo_counter) self.explore_point( triangle.p3, triangle, center.pc, os.path.join(img_dir, "%d.png" % photo_counter), ) photo_counter += 1 print("Center point, counter=%d" % photo_counter) self.explore_point( triangle.pc, triangle, center.pc, os.path.join(img_dir, "%d.png" % photo_counter), )
def matrix_from_rotation_axis_angle(axis: Vector3r, angle: float, is_degrees: bool = False) -> np.ndarray: # ref.: https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle if is_degrees: angle = np.deg2rad(angle) x, y, z = axis.to_numpy_array() ca = np.cos(angle) sa = np.sin(angle) omca = 1 - ca return np.array( [ [ca + (x ** 2) * omca, x * y * omca - z * sa, x * z * omca + y * sa], [y * x * omca + z * sa, ca + (y ** 2) * omca, y * z * omca - x * sa], [z * x * omca - y * sa, z * y * omca + x * sa, ca + (z ** 2) * omca], ] )
def explore(self): """Traverse the concentric circles in turn by the strategy of iteratively halving the radius of the circle """ if not self._safety_surface["type"] == "circle": return img_dir = self._config["image_dir"] # Arm and take off self._uav.armDisarm(True) self._uav.takeoffAsync().join() self._move_to(Vector3r(0, 0, -60)) # Explore center point and points on edge points_per_round = self.generate_explore_views() # self.show_explore_path(points_per_round) center = points_per_round[0][0] root_triangle = Triangle(center, center, center, None, True) point_counter = 1 print("Center point, counter=1") self.explore_point( center, root_triangle, root_triangle.pc, os.path.join(img_dir, "%d.png" % point_counter), ) for point in points_per_round[-1]: point_counter += 1 print("round %d, point, counter=%d" % (len(points_per_round) - 1, point_counter)) self.explore_point( point, root_triangle, root_triangle.pc, os.path.join(img_dir, "%d.png" % point_counter), ) """Explore circle 2, 1, 3 successively""" point_counter = self.between(0, 4, points_per_round, 2, point_counter, img_dir) point_counter = self.between(0, 2, points_per_round, 1, point_counter, img_dir) point_counter = self.between(2, 4, points_per_round, 3, point_counter, img_dir)
def setRandomPose(client, center, offset_range, rotation_range): random_vector = np.random.random_sample((2, 1)) unit_vector = random_vector / np.linalg.norm(random_vector) displacement_vector = (offset_range[1] - offset_range[0]) * ( np.random.random_sample()) * unit_vector + offset_range[0] displacement_vector = displacement_vector.reshape(-1, ) position = Vector3r(center.position.x_val + displacement_vector[0], center.position.y_val + displacement_vector[1], center.position.z_val) rotation = (rotation_range[1] - rotation_range[0] ) * np.random.random_sample() + rotation_range[ 0] #2*np.sin(2*np.pi*np.random.random_sample()) orientation = Quaternionr(z_val=rotation) pose = Pose(position_val=position, orientation_val=orientation) client.simSetObjectPose("pallet", pose, True) return pose, displacement_vector, rotation
def explore(self): """Explore the place of interest from the outer circle to the inner circle""" if not self._safety_surface["type"] == "circle": return img_dir = self._config["image_dir"] # Arm and take off self._uav.armDisarm(True) self._uav.takeoffAsync().join() self._move_to(Vector3r(0, 0, -60)) # Explore center point points_per_round = self.generate_explore_views() self.show_explore_path(points_per_round) center = points_per_round[0][0] root_triangle = Triangle(center, center, center, None, True) photo_counter = 1 print("Center point, counter=1") self.explore_point(center, root_triangle, root_triangle.pc, os.path.join(img_dir, "1.png")) # Explore the outer circle for point in points_per_round[-1]: photo_counter += 1 print("round %d, point, counter=%d" % (len(points_per_round) - 1, photo_counter)) self.explore_point( point, root_triangle, root_triangle.pc, os.path.join(img_dir, "%d.png") % photo_counter, ) # Explore other points from the outer circle to the inner circle for i in range(len(points_per_round) - 2, 0, -1): for point in points_per_round[i]: photo_counter += 1 print("round %d, point, counter=%d" % (i, photo_counter)) tmp_triangle = self.__generate_smallest_triangle( point, center, points_per_round[i + 1]) self.explore_point( point, tmp_triangle, tmp_triangle.pc, os.path.join(img_dir, "%d.png" % photo_counter), )
def _observe_at_view(self, view, img_path): """Fly to given position and take a photo at given orientation.""" position = view["position"] position = (Vector3r(position[0], position[1], position[2]) if type(position) == list else position) print("Observe (%f, %f, %f) with yaw %f and pitch %f" % ( position.x_val, position.y_val, position.z_val, view["yaw"] / math.pi * 180, view["pitch"] / math.pi * 180, )) self._move_to(position, yaw=view["yaw"] / math.pi * 180) # self._uav.hoverAsync().join() # time.sleep(5) self._uav.simSetCameraOrientation( "0", airsim.to_quaternion(view["pitch"], 0.0, 0.0)) png = self._uav.simGetImage("0", airsim.ImageType.Scene) ReconstructionNavigator.mkdir(os.path.dirname(img_path)) airsim.write_file(img_path, png)
def capture_LIDAR_depth(self, p, q): """ p: A 3-element NumPy array, the position. q: A 4-element NumPy array, quaternion, w is the last element. """ faces = [0, 1, 2, -1] # Front, right, back, left. depthList = [] q0 = Quaternionr(q[0], q[1], q[2], q[3]) for face in faces: # Compose a AirSim Pose object. yaw = np.pi / 2 * face yq = to_quaternion(0, 0, yaw) # q = to_quaternion( e[0], e[1], e[2] ) q1 = yq * q0 pose = Pose(Vector3r(p[0], p[1], p[2]), q1) # Change the vehicle pose. self.set_vehicle_pose(pose) # Get the image. for i in range(self.maxTrial): depth, _ = self.get_depth_campos() if depth is None: print("Fail for trail %d on face %d. " % (i, face)) continue if (depth is None): raise Exception("Could not get depth image for face %d. " % (face)) # Get a valid depth. depthList.append(depth) return depthList
def pose_from_meshroom_to_airsim(meshroom_pose): assert len(meshroom_pose.center) == 3 and len( meshroom_pose.rotation) == 9, meshroom_pose xyzw = MeshroomTransform.rotation(meshroom_pose.rotation, as_xyzw_quaternion=True) return Pose(Vector3r(*meshroom_pose.center), Quaternionr(*xyzw))
def __init__(self, vehicle_names, image_mask_FC_FR_FL=[True, False, False], reward_function=car_racing_rewarding_function, mode="rgb_normal"): self.vehicle_names = vehicle_names # Set reward function self.reward_function = reward_function # Set Environment Return options self.mode = mode self.mode # Set Drive Commands to zero initially self._throttle = dict.fromkeys( self.vehicle_names, 0) # Used as a constant gain factor for the action throttle. self._steering = dict.fromkeys( self.vehicle_names, 0) # Each action lasts this many seconds self._brake = dict.fromkeys(self.vehicle_names, 0) self.THROTTLE_INC = .10 self.THROTTLE_DEC = -.10 self.BRAKE_INC = .10 self.BRAKE_DEC = -.20 self.STEER_LEFT_INC = -.10 self.STEER_RIGHT_INC = .10 # The number of INERTIAL state variables to keep track of self.count_inertial_state_variables = 15 # Posx, Posy, PosZ, Vx, Vy, Vz, Ax, Ay, Az, AngVelx, AngVely, AngVelz, AngAccx, AngAccy, AngAccz # Throttle up, throttle down, increase brake, decrease break, left_steer, right_steer, No action self.count_car_actions = 7 # Initialize the current inertial state to zero self.current_inertial_states = dict.fromkeys( self.vehicle_names, np.array(np.zeros(self.count_inertial_state_variables))) # Initialize the IMAGE variables -- We Take in Front Center, Right, Left self.images_rgb = dict.fromkeys(self.vehicle_names, 0) self.images_rgba = dict.fromkeys(self.vehicle_names, 0) self.image_mask_rgb = np.array( [[0 + 3 * i, 1 + 3 * i, 2 + 3 * i] for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1) self.image_mask_rgba = np.array( [[0 + 4 * i, 1 + 4 * i, 2 + 4 * i] for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1) self.image_mask_FC_FR_FL = image_mask_FC_FR_FL self.initial_position = dict.fromkeys(self.vehicle_names, 0) self.initial_velocity = dict.fromkeys(self.vehicle_names, 0) # Connect to the AirSim simulator and begin: print('Initializing Car Client') self.client = client.CarClient() self.client.confirmConnection() for vn in self.vehicle_names: self.client.enableApiControl(True, vn) orien = Vector3r(0, 0, 0) self.client.simSetCameraOrientation(0, orien.to_Quaternionr(), vehicle_name=vn) #radians orien = Vector3r(0, .12, -np.pi / 9) self.client.simSetCameraOrientation(1, orien, vehicle_name=vn) orien = Vector3r(0, .12, np.pi / 9) self.client.simSetCameraOrientation(2, orien, vehicle_name=vn) # Reset Collion Flags print('Initialization Complete!') # Timing Operations Initialize self.dt = 0 self.tic = 0 self.toc = 0
def __init__(self, vehicle_name="Car1", action_duration=.08, image_mask_FC_FR_FL=[True, True, True], sim_mode="rgb", IMG_HEIGHT=128, IMG_WIDTH=128, IMG_STEP=3): # Set Environment Return options self.action_duration = action_duration self.mode = sim_mode # Set Drive Commands to zero initially self._throttle = 0 # Used as a constant gain factor for the action throttle. self._steering = 0 # Each action lasts this many seconds self._brake = 0 # Simulator Image setup self.IMG_HEIGHT = IMG_HEIGHT self.IMG_WIDTH = IMG_WIDTH isRGB = False IMG_CHANNELS = 1 if 'rgb' in self.mode: isRGB = True IMG_CHANNELS = 3 isNormal = False if 'normal' in self.mode: isNormal = True self.IMG_CHANNELS = IMG_CHANNELS self.IMG_STEP = IMG_STEP self.IMG_VIEWS = np.sum(np.array(image_mask_FC_FR_FL, dtype=np.int)) # Initialize the container that holds the sequence of images from the simulator self.obs4 = np.zeros( (self.IMG_HEIGHT, self.IMG_WIDTH, self.IMG_CHANNELS * self.IMG_STEP * self.IMG_VIEWS)) # The number of INERTIAL state variables to keep track of self.count_inertial_state_variables = 15 # Posx, Posy, PosZ, Vx, Vy, Vz, Ax, Ay, Az, AngVelx, AngVely, AngVelz, AngAccx, AngAccy, AngAccz # Throttle up, throttle down, increase brake, decrease break, left_steer, right_steer, No action self.count_car_actions = 7 # Initialize the current inertial state to zero self.current_inertial_state = np.array( np.zeros(self.count_inertial_state_variables)) # Initialize the IMAGE variables -- We Take in Front Center, Right, Left self.images_rgb = None self.images_rgba = None self.image_mask_rgb = np.array( [[0 + 3 * i, 1 + 3 * i, 2 + 3 * i] for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1) self.image_mask_rgba = np.array( [[0 + 4 * i, 1 + 4 * i, 2 + 4 * i] for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1) self.image_mask_FC_FR_FL = image_mask_FC_FR_FL # Connect to the AirSim simulator and begin: print('Initializing Car Client') self.client = client.CarClient() self.client.confirmConnection() self.client.enableApiControl(True) print('Initialization DONE!') print("Setting Camera Views") orien = Vector3r(0, 0, 0) self.client.simSetCameraOrientation("0", orien) #radians orien = Vector3r(0, .12, -np.pi / 9) self.client.simSetCameraOrientation("1", orien) orien = Vector3r(0, .12, np.pi / 9) self.client.simSetCameraOrientation("2", orien) # Reset Collion Flags print("Setting Camera Views DONE!") # Set up GUI Video Feeder self.gui_data = {'obs': None, 'state': None, 'meta': None} self.vehicle_name = vehicle_name num_video_feeds = np.sum( np.array(self.image_mask_FC_FR_FL, dtype=np.int)) * IMG_STEP GUIConn, self.simEnvDataConn = multiprocessing.Pipe() self.app = AirSimGUI.CarGUI(GUIConn, vehicle_names=[vehicle_name], num_video_feeds=num_video_feeds, isRGB=isRGB, isNormal=isNormal) # Timing Operations Initialize self.time_to_do_action = 0 self.time_to_grab_images = 0 self.time_to_grab_states = 0 self.time_to_calc_reward = 0 self.time_to_step = 0 self.extra_metadata = None