def reset(self, back_up, itr=None, cond=None, rep=None, record=True):
# self._agent.execute_control(None) # stop the car
assert(itr is not None and cond is not None and rep is not None)
# TODO add sim backup
### back car up straight and slow
if back_up:
sample = Sample(meta_data=params, T=2)
sample.set_U([np.random.uniform(*self.wp['back_up']['cmd_steer'])], t=0, sub_control='cmd_steer')
sample.set_U([self.wp['back_up']['cmd_vel']], t=0, sub_control='cmd_vel')
u = sample.get_U(t=0)
if self._agent.sim:
if self._agent.sim_coll and self._agent.sim_last_coll:
self._logger.info('Resetting the car')
self._agent.execute_control(None, reset=True)
elif self._agent.sim_coll:
# TODO add backup logic for not sim
self._logger.info('Backing the car up')
for _ in xrange(int(self.wp['back_up']['duration'] / params['dt'])):
self._agent.execute_control(u)
if self._agent.sim_coll:
break
for _ in xrange(int(1.0 / params['dt'])):
self._agent.execute_control(None)
else:
self._logger.info('Backing the car up')
start = time.time()
while time.time() - start < self.wp['back_up']['duration']:
self._agent.execute_control(u)
self._agent.execute_control(None)
time.sleep(1.0)
else:
self._agent.execute_control(None, reset=True)
### TODO add a flag
self._ros_reset_pub.publish(std_msgs.msg.Empty())
### record
if record:
self._start_record_bag(itr, cond, rep)
if not self._agent.sim:
### reset indicators
self._ros_collision.get()
self.num_collisions = 0
def act(self, x, obs, t, noise, ref_traj=None):
### create desired path (radiates outward)
T, dt, theta, speed = self._T, self._meta_data[
'dt'], self.theta, self.speed
traj = Sample(meta_data=self._meta_data, T=T)
### initialize
traj.set_X(x, t=0)
traj.set_O(obs, t=0)
### create desired path (radiates outward)
linearvel = [speed * np.cos(theta), speed * np.sin(theta)]
for t in xrange(T - 1):
x_t = traj.get_X(t=t)
x_tp1 = self._dynamics.evolve(x_t, linearvel)
traj.set_X(x_tp1, t=t + 1)
traj.set_U(linearvel, t=t)
traj.set_U(linearvel, t=-1)
self._curr_traj = traj
u = traj.get_U(t=0)
return u + noise.sample(u)