def sample_action(self, current_state, explore=True):
# generate sensor data
array_laser = utils.remapping_laser_data(current_state.laserScan)
sensor = Variable(
torch.FloatTensor(np.reshape(array_laser, (1, self.sensor_dim))))
# generate target data
target_polar = utils.target_transform(current_state)
target = Variable(
torch.FloatTensor(np.reshape(target_polar, (1, self.target_dim))))
# generate action
action = self.actor_ca(sensor=sensor, target=target).cpu().data.numpy()
if explore and random.uniform(0, 1) > 0.7 and self.train_type is not 3:
action = action + self.noise.sample()
#action[0][0] = random.uniform(-1,1)
#action[0][1] = random.uniform(-1,1)
# constrain the action
action[0][0] = utils.constrain_actions(action[0][0], 1)
action[0][1] = utils.constrain_actions(action[0][1], 1)
return action[0][0], action[0][1]
def navigation(self, current_state):
# generate sensor data
array_laser = utils.remapping_laser_data(current_state.laserScan)
sensor = Variable(torch.FloatTensor(np.reshape(array_laser, (1, self.sensor_dim))))
# generate target data
target_polar = utils.target_transform(current_state)
target = Variable(torch.FloatTensor(np.reshape(target_polar, (1, self.target_dim))))
# generate action
target_driven_action = self.differential_driver.run(x=current_state.desired_x, y=current_state.desired_y)
collision_avoidance_action = self.actor_ca(sensor=sensor, target=target).cpu().data.numpy()
predict_state = self.evaluation_net.predict_state(array_laser.reshape(1, -1))
# Collision avoidance ratio
ratio = min(float(torch.kthvalue(sensor,1)[0]) / (-3.5), 1)
# print ratio
final_action = []
for i in range(2):
final_action.append((1.0 - ratio) * target_driven_action[0][i] + ratio * collision_avoidance_action[0][i] )
# print final_action
# constrain the action
final_action[0] = utils.constrain_actions(final_action[0], 1)
final_action[1] = utils.constrain_actions(final_action[1], 1)
# print final_action
return final_action[0], final_action[1]
def sample_action(self, cx, cy, cow, cox, coy, coz, tx, ty, tow, tox, toy, toz, explore=True):
state = Variable(torch.FloatTensor(np.transpose(np.reshape([cx,cy,cow,cox,coy,coz,tx,ty,tow,tox,toy,toz], [6*2, 1]))))
action = self.actor(state).cpu().data.numpy()
# add some noise to the action
action[0][0] = utils.constrain_actions(action[0][0], 2)
action[0][1] = utils.constrain_actions(action[0][1], 1)
if explore:
if random.uniform(0,1) > 0.7:
#action = action + self.noise.sample()
action[0][0] = random.uniform(0,2)
action[0][1] = random.uniform(-1,1)
return action[0][0], action[0][1]
def sample_action(self, current_state, explore=True):
# generate states
cvx, cvy = utils.vector_normalization(current_state.target_x,
current_state.target_y,
current_state.current_x,
current_state.current_y)
# reshape is needed here, because single sample and batch sample should be both 2-dim
state = Variable(
torch.FloatTensor(
np.transpose(
np.reshape([
cvx, cvy, current_state.orientation_w,
current_state.orientation_x,
current_state.orientation_y,
current_state.orientation_z
], (self.state_dim, 1)))))
'''
state = Variable(torch.FloatTensor(np.transpose(np.reshape([current_state.current_x, current_state.current_y,
current_state.target_x, current_state.target_y,
current_state.orientation_w,current_state.orientation_x,
current_state.orientation_y,current_state.orientation_z],
(self.state_dim, 1)))))
'''
# generate sensor data
array_laser = np.array(current_state.laserScan)
where_inf = np.isinf(array_laser)
array_laser[where_inf] = 3.5
sensor = Variable(
torch.FloatTensor(np.reshape(array_laser, (1, self.sensor_dim))))
# generate action
action = self.actor(state=state, sensor=sensor).cpu().data.numpy()
# amplify the linear speed
#action[0][0] = 2*action[0][0]
if explore:
if random.uniform(0, 1) > 0.7:
#action = action + self.noise.sample()
action[0][0] = random.uniform(-1, 1)
action[0][1] = random.uniform(-1, 1)
# constrain the action
action[0][0] = utils.constrain_actions(action[0][0], 1)
action[0][1] = utils.constrain_actions(action[0][1], 1)
return action[0][0], action[0][1]
def sample_action(self, current_state, laser_data, explore=True):
# reshape is needed here, because single sample and batch sample should be both 2-dim
state = Variable(
torch.FloatTensor(
np.transpose(
np.reshape(
[current_state.desired_x, current_state.desired_y],
(self.state_dim, 1)))))
# generate sensor data
array_laser_1 = utils.remapping_laser_data(laser_data[0])
array_laser_2 = utils.remapping_laser_data(laser_data[1])
array_laser_3 = utils.remapping_laser_data(current_state.laserScan)
sensor_1 = Variable(
torch.FloatTensor(np.reshape(array_laser_1, (1, self.sensor_dim))))
sensor_2 = Variable(
torch.FloatTensor(np.reshape(array_laser_2, (1, self.sensor_dim))))
sensor_3 = Variable(
torch.FloatTensor(np.reshape(array_laser_3, (1, self.sensor_dim))))
sensor = torch.cat([sensor_1, sensor_2, sensor_3], 1)
# generate action
if self.train_type == 1:
action = self.actor_td(state=state).cpu().data.numpy()
elif self.train_type == 2:
action = self.actor_ca(sensor=sensor).cpu().data.numpy()
else:
action = self.differential_driver.run(x=current_state.desired_x,
y=current_state.desired_y)
if explore and random.uniform(0, 1) > 0.7 and self.train_type is not 3:
action = action + self.noise.sample()
#action[0][0] = random.uniform(-1,1)
#action[0][1] = random.uniform(-1,1)
# constrain the action
action[0][0] = utils.constrain_actions(action[0][0], 1)
action[0][1] = utils.constrain_actions(action[0][1], 1)
return action[0][0], action[0][1]