def __init__(self, params={}): self._yaw_limits = params['yaw_limits'] self._obs_shape = params['obs_shape'] self._horizon = params['horizon'] self.action_spec = OrderedDict() self.action_selection_spec = OrderedDict() self.observation_vec_spec = OrderedDict() self.goal_spec = OrderedDict() self.action_spec['yaw'] = Box(low=-180, high=180) self.action_space = Box(low=np.array([self.action_spec['yaw'].low[0]]), high=np.array([self.action_spec['yaw'].high[0]])) self.action_selection_spec['yaw'] = Box(low=self._yaw_limits[0], high=self._yaw_limits[1]) self.action_selection_space = Box(low = np.array([self.action_selection_spec['yaw'].low[0]]), high = np.array([self.action_selection_spec['yaw'].high[0]])) self.observation_im_space = Box(low=0, high=255, shape=self._obs_shape) self.observation_vec_spec['coll'] = Discrete(1) self.spec = EnvSpec( observation_im_space=self.observation_im_space, action_space=self.action_space, action_selection_space=self.action_selection_space, observation_vec_spec=self.observation_vec_spec, action_spec=self.action_spec, action_selection_spec=self.action_selection_spec, goal_spec=self.goal_spec )
def _setup_spec(self): action_spec = OrderedDict() action_selection_spec = OrderedDict() observation_vec_spec = OrderedDict() action_spec['steer'] = Box(low=-1., high=1.) action_spec['motor'] = Box(low=-1., high=1.) action_space = Box(low=np.array([v.low[0] for k, v in action_spec.items()]), high=np.array([v.high[0] for k, v in action_spec.items()])) action_selection_spec['steer'] = Box(low=-1., high=1.) action_selection_spec['motor'] = Box(low=0.3, high=1.) # min 0.3--> 1.4m/s == 4km/h, need 1km/h to detect collisions action_selection_space = Box(low=np.array([v.low[0] for k, v in action_selection_spec.items()]), high=np.array([v.high[0] for k, v in action_selection_spec.items()])) assert (np.logical_and(action_selection_space.low >= action_space.low, action_selection_space.high <= action_space.high).all()) num_channels = 0 for camera_name in self._params['cameras']: camera_params = self._params[camera_name] assert(camera_params['postprocessing'] is not None) if camera_params['include_in_obs']: if camera_params['postprocessing'] == 'SceneFinal': num_channels += 1 if camera_params.get('grayscale', False) else 3 else: num_channels += 1 observation_im_space = Box(low=0, high=255, shape=list(self._params['camera_size']) + [num_channels]) observation_vec_spec['coll'] = Discrete(1) observation_vec_spec['coll_car'] = Discrete(1) observation_vec_spec['coll_ped'] = Discrete(1) observation_vec_spec['coll_oth'] = Discrete(1) observation_vec_spec['heading'] = Box(low=-180., high=180.) observation_vec_spec['speed'] = Box(low=-30., high=30.) observation_vec_spec['accel_x'] = Box(low=-100., high=100.) observation_vec_spec['accel_y'] = Box(low=-100., high=100.) observation_vec_spec['accel_z'] = Box(low=-100., high=100.) goal_spec = self._setup_goal_spec() self.action_spec, self.action_selection_spec, self.observation_vec_spec, self.goal_spec, \ self.action_space, self.action_selection_space, self.observation_im_space = \ action_spec, action_selection_spec, observation_vec_spec, goal_spec, \ action_space, action_selection_space, observation_im_space self.spec = EnvSpec( observation_im_space=observation_im_space, action_space=action_space, action_selection_space=action_selection_space, observation_vec_spec=observation_vec_spec, action_spec=action_spec, action_selection_spec=action_selection_spec, goal_spec=goal_spec)
def __init__(self, params={}): self.observation_im_space = None self.action_space = None self.action_selection_space = None self.observation_vec_spec = None self.action_spec = None self.action_selection_spec = None self.goal_spec = None self.spec = EnvSpec(observation_im_space=self.observation_im_space, action_space=self.action_space, action_selection_space=self.action_selection_space, observation_vec_spec=self.observation_vec_spec, action_spec=self.action_spec, action_selection_spec=self.action_selection_spec, goal_spec=self.goal_spec)
def __init__(self, params={}, gpu_count=0): DroneNavigateEnv.__init__(self, params, gpu_count) self._obs_shape = params['obs_shape'] self._yaw_limits = params['yaw_limits'] self._horizon = params['horizon'] self._model_id = params['model_id'] self._setup_spec() assert (self.observation_im_space.shape[-1] == 1 or self.observation_im_space.shape[-1] == 3) self.spec = EnvSpec(self.observation_im_space, self.action_space, self.action_selection_space, self.observation_vec_spec, self.action_spec, self.action_selection_spec, self.goal_spec)
def __init__(self, params={}): params.setdefault('dt', 0.25) params.setdefault('horizon', int(5. * 60. / params['dt'])) # 5 minutes worth params.setdefault('ros_namespace', '/rccar/') params.setdefault('obs_shape', (36, 64, 1)) params.setdefault('steer_limits', [-0.9, 0.9]) params.setdefault('speed_limits', [0.2, 0.2]) params.setdefault('backup_motor', -0.22) params.setdefault('backup_duration', 1.6) params.setdefault('backup_steer_range', (-0.8, 0.8)) params.setdefault('press_enter_on_reset', False) self._use_vel = True self._obs_shape = params['obs_shape'] self._steer_limits = params['steer_limits'] self._speed_limits = params['speed_limits'] self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1] and self._use_vel) self._collision_reward = params['collision_reward'] self._collision_reward_only = params['collision_reward_only'] self._dt = params['dt'] self.horizon = params['horizon'] self._setup_spec() assert (self.observation_im_space.shape[-1] == 1 or self.observation_im_space.shape[-1] == 3) self.spec = EnvSpec(observation_im_space=self.observation_im_space, action_space=self.action_space, action_selection_space=self.action_selection_space, observation_vec_spec=self.observation_vec_spec, action_spec=self.action_spec, action_selection_spec=self.action_selection_spec, goal_spec=self.goal_spec) self._last_step_time = None self._is_collision = False self._backup_motor = params['backup_motor'] self._backup_duration = params['backup_duration'] self._backup_steer_range = params['backup_steer_range'] self._press_enter_on_reset = params['press_enter_on_reset'] ### ROS if not ROS_IMPORTED: logger.warn('ROS not imported') return rospy.init_node('RWrccarEnv', anonymous=True) rospy.sleep(1) self._ros_namespace = params['ros_namespace'] self._ros_topics_and_types = dict([ ('camera/image_raw/compressed', sensor_msgs.msg.CompressedImage), ('mode', std_msgs.msg.Int32), ('steer', std_msgs.msg.Float32), ('motor', std_msgs.msg.Float32), ('encoder/left', std_msgs.msg.Float32), ('encoder/right', std_msgs.msg.Float32), ('encoder/both', std_msgs.msg.Float32), ('encoder/error', std_msgs.msg.Int32), ('orientation/quat', geometry_msgs.msg.Quaternion), ('orientation/rpy', geometry_msgs.msg.Vector3), ('imu', geometry_msgs.msg.Accel), ('collision/all', std_msgs.msg.Int32), ('collision/flip', std_msgs.msg.Int32), ('collision/jolt', std_msgs.msg.Int32), ('collision/stuck', std_msgs.msg.Int32), ('collision/bumper', std_msgs.msg.Int32), ('cmd/steer', std_msgs.msg.Float32), ('cmd/motor', std_msgs.msg.Float32), ('cmd/vel', std_msgs.msg.Float32) ]) self._ros_msgs = dict() self._ros_msg_times = dict() for topic, type in self._ros_topics_and_types.items(): rospy.Subscriber(self._ros_namespace + topic, type, self.ros_msg_update, (topic, )) self._ros_steer_pub = rospy.Publisher(self._ros_namespace + 'cmd/steer', std_msgs.msg.Float32, queue_size=10) self._ros_vel_pub = rospy.Publisher(self._ros_namespace + 'cmd/vel', std_msgs.msg.Float32, queue_size=10) self._ros_motor_pub = rospy.Publisher(self._ros_namespace + 'cmd/motor', std_msgs.msg.Float32, queue_size=10) self._ros_pid_enable_pub = rospy.Publisher(self._ros_namespace + 'pid/enable', std_msgs.msg.Empty, queue_size=10) self._ros_pid_disable_pub = rospy.Publisher(self._ros_namespace + 'pid/disable', std_msgs.msg.Empty, queue_size=10) self._ros_rolloutbag = RolloutRosbag() self._t = 0
def __init__(self, params={}): params.setdefault('dt', 0.25) params.setdefault('horizon', int(5. * 60. / params['dt'])) # 5 minutes worth params.setdefault('ros_namespace', '/crazyflie/') params.setdefault('obs_shape', (72, 96, 1)) params.setdefault('yaw_limits', [-120, 120]) #default yaw rate range params.setdefault('fixed_alt', 0.4) params.setdefault('fixed_velocity_range', [0.4, 0.4]) params.setdefault('press_enter_on_reset', False) params.setdefault('prompt_save_rollout_on_coll', False) params.setdefault('enable_adjustment_on_start', True) params.setdefault('use_joy_commands', True) params.setdefault('joy_start_btn', 1) #A params.setdefault('joy_stop_btn', 2) #B params.setdefault('joy_coll_stop_btn', 0) #X params.setdefault('joy_trash_rollout_btn', 3) # Y params.setdefault('joy_topic', '/joy') params.setdefault('collision_reward', 1) params.setdefault('collision_reward_only', True) self._obs_shape = params['obs_shape'] self._yaw_limits = params['yaw_limits'] self._fixed_alt = params['fixed_alt'] self._collision_reward = params['collision_reward'] self._collision_reward_only = params['collision_reward_only'] self._fixed_velocity_range = params['fixed_velocity_range'] self._fixed_velocity = np.random.uniform(self._fixed_velocity_range[0], self._fixed_velocity_range[1]) self._dt = params['dt'] self.horizon = params['horizon'] # start stop and pause self._enable_adjustment_on_start = params['enable_adjustment_on_start'] self._use_joy_commands = params['use_joy_commands'] self._joy_topic = params['joy_topic'] self._joy_stop_btn = params['joy_stop_btn'] self._joy_coll_stop_btn = params['joy_coll_stop_btn'] self._joy_start_btn = params['joy_start_btn'] self._joy_trash_rollout_btn = params['joy_trash_rollout_btn'] self._press_enter_on_reset = params['press_enter_on_reset'] self._prompt_save_rollout_on_coll = params[ 'prompt_save_rollout_on_coll'] self._start_pressed = False self._stop_pressed = False self._trash_rollout = False self._coll_stop_pressed = False self._curr_joy = None self._curr_motion = crazyflie.msg.CFMotion() self._setup_spec() assert (self.observation_im_space.shape[-1] == 1 or self.observation_im_space.shape[-1] == 3) self.spec = EnvSpec(observation_im_space=self.observation_im_space, action_space=self.action_space, action_selection_space=self.action_selection_space, observation_vec_spec=self.observation_vec_spec, action_spec=self.action_spec, action_selection_spec=self.action_selection_spec, goal_spec=self.goal_spec) self._last_step_time = None self._is_collision = False rospy.init_node('CrazyflieEnv', anonymous=True) time.sleep(0.5) self._ros_namespace = params['ros_namespace'] self._ros_topics_and_types = dict([ ('cf/0/image', sensor_msgs.msg.CompressedImage), ('cf/0/data', crazyflie.msg.CFData), ('cf/0/coll', std_msgs.msg.Bool), ('cf/0/motion', crazyflie.msg.CFMotion) ]) self._ros_msgs = dict() self._ros_msg_times = dict() for topic, type in self._ros_topics_and_types.items(): rospy.Subscriber(topic, type, self.ros_msg_update, (topic, )) self._ros_motion_pub = rospy.Publisher("/cf/0/motion", crazyflie.msg.CFMotion, queue_size=10) self._ros_command_pub = rospy.Publisher("/cf/0/command", crazyflie.msg.CFCommand, queue_size=10) self._ros_stop_pub = rospy.Publisher('/joystop', crazyflie.msg.JoyStop, queue_size=10) if self._use_joy_commands: logger.debug("Environment using joystick commands") self._ros_joy_sub = rospy.Subscriber(self._joy_topic, sensor_msgs.msg.Joy, self._joy_cb) # I don't think this is needed # self._ros_pid_enable_pub = rospy.Publisher(self._ros_namespace + 'pid/enable', std_msgs.msg.Empty, # queue_size=10) # self._ros_pid_disable_pub = rospy.Publisher(self._ros_namespace + 'pid/disable', std_msgs.msg.Empty, # queue_size=10) self._ros_rolloutbag = RolloutRosbag() self._t = 0 self.suppress_output = False self.resetting = False self._send_override = False # set true only when resetting but still wanting to send background thread motion commands threading.Thread(target=self._background_thread).start() time.sleep(1.0) #waiting for some messages before resetting self.delete_this_variable = 0
def __init__(self, params={}): self._params = params if 'random_seed' in self._params: np.random.seed(self._params['random_seed']) self._use_vel = self._params.get('use_vel', True) self._run_as_task = self._params.get('run_as_task', False) self._do_back_up = self._params.get('do_back_up', False) self._use_depth = self._params.get('use_depth', False) self._use_back_cam = self._params.get('use_back_cam', False) self._collision_reward_only = self._params.get('collision_reward_only', False) self._collision_reward = self._params.get('collision_reward', -10.0) self._obs_shape = self._params.get('obs_shape', (64, 36)) self._steer_limits = params.get('steer_limits', (-30., 30.)) self._speed_limits = params.get('speed_limits', (-4.0, 4.0)) self._motor_limits = params.get('motor_limits', (-0.5, 0.5)) self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1] and self._use_vel) if not self._params.get('visualize', False): loadPrcFileData('', 'window-type offscreen') # Defines base, render, loader try: ShowBase() except: pass base.setBackgroundColor(0.0, 0.0, 0.0, 1) # World self._worldNP = render.attachNewNode('World') self._world = BulletWorld() self._world.setGravity(Vec3(0, 0, -9.81)) self._dt = params.get('dt', 0.25) self._step = 0.05 # Vehicle shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25)) ts = TransformState.makePos(Point3(0., 0., 0.25)) self._vehicle_node = BulletRigidBodyNode('Vehicle') self._vehicle_node.addShape(shape, ts) self._mass = self._params.get('mass', 10.) self._vehicle_node.setMass(self._mass) self._vehicle_node.setDeactivationEnabled(False) self._vehicle_node.setCcdSweptSphereRadius(1.0) self._vehicle_node.setCcdMotionThreshold(1e-7) self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node) self._world.attachRigidBody(self._vehicle_node) self._vehicle = BulletVehicle(self._world, self._vehicle_node) self._vehicle.setCoordinateSystem(ZUp) self._world.attachVehicle(self._vehicle) self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07) self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07) # Camera size = self._params.get('size', [160, 90]) hfov = self._params.get('hfov', 120) near_far = self._params.get('near_far', [0.1, 100.]) self._camera_sensor = Panda3dCameraSensor(base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='front cam') self._camera_node = self._camera_sensor.cam self._camera_node.setPos(0.0, 0.5, 0.375) self._camera_node.lookAt(0.0, 6.0, 0.0) self._camera_node.reparentTo(self._vehicle_pointer) if self._use_back_cam: self._back_camera_sensor = Panda3dCameraSensor( base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='back cam') self._back_camera_node = self._back_camera_sensor.cam self._back_camera_node.setPos(0.0, -0.5, 0.375) self._back_camera_node.lookAt(0.0, -6.0, 0.0) self._back_camera_node.reparentTo(self._vehicle_pointer) # Car Simulator self._des_vel = None self._setup() # Input self.accept('escape', self._doExit) self.accept('r', self.reset) self.accept('f1', self._toggleWireframe) self.accept('f2', self._toggleTexture) self.accept('f3', self._view_image) self.accept('f5', self._doScreenshot) self.accept('q', self._forward_0) self.accept('w', self._forward_1) self.accept('e', self._forward_2) self.accept('a', self._left) self.accept('s', self._stop) self.accept('x', self._backward) self.accept('d', self._right) self.accept('m', self._mark) self._steering = 0.0 # degree self._engineForce = 0.0 self._brakeForce = 0.0 self._env_time_step = 0 self._p = self._params.get('p', 1.25) self._d = self._params.get('d', 0.0) self._last_err = 0.0 self._curr_time = 0.0 self._accelClamp = self._params.get('accelClamp', 0.5) self._engineClamp = self._accelClamp * self._mass self._collision = False self._setup_spec() self.spec = EnvSpec(observation_im_space=self.observation_im_space, action_space=self.action_space, action_selection_space=self.action_selection_space, observation_vec_spec=self.observation_vec_spec, action_spec=self.action_spec, action_selection_spec=self.action_selection_spec, goal_spec=self.goal_spec) if self._run_as_task: self._mark_d = 0.0 taskMgr.add(self._update_task, 'updateWorld') base.run()