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()