def start(self):
self.vrep_start()
t = vrep.simxGetLastCmdTime(self.clientID)
vrep.simxSynchronousTrigger(self.clientID)
self.controller([0.0, 0.0])
while vrep.simxGetLastCmdTime(self.clientID) - t < self.dt:
lrf_bin = vrep.simxGetStringSignal(self.clientID, 'hokuyo_data',
vrep.simx_opmode_streaming)[1]
pose = vrep.simxGetObjectPosition(self.clientID, self.body_handle,
-1,
vrep.simx_opmode_streaming)[1]
def get_measurement(self):
lrf_bin = vrep.simxGetStringSignal(
self.clientID,
'measurement',
vrep.simx_opmode_blocking
)[1]
try:
lrf = array(vrep.simxUnpackFloats(lrf_bin), dtype=float)/3.5
lrf = npmax(reshape(lrf, [-1,10]), 1)
except:
lrf = None
return lrf
from ou_noise import OUNoise
# from environment.vrep_env import Env
from environment.env_modules import vrep
import time
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
lidar_list = ['fastHokuyo_sensor1', 'fastHokuyo_sensor2', 'fastHokuyo_sensor3']
lidar_handles = {}
for name in lidar_list:
lidar_handles[name] = vrep.simxGetObjectHandle(
clientID, name, vrep.simx_opmode_blocking)[1]
# env = Env(19997)
vrep.simxSynchronous(clientID, True)
time.sleep(0.5)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
while vrep.simxGetStringSignal(clientID, "lidar",
vrep.simx_opmode_blocking)[0] != 0:
vrep.simxSynchronousTrigger(clientID)
data = vrep.simxGetStringSignal(clientID, "lidar",
vrep.simx_opmode_blocking)[1]
measuredData = vrep.simxUnpackFloats(data)
lidar = []
for i in range(18):
lidar.append(min(measuredData[57 * i:57 * (i + 1)]))
r = np.asarray(measuredData)
r1 = np.asarray(lidar)
theta = np.linspace(0.0, np.pi * 2, 1026)
theta1 = np.linspace(0.0, np.pi * 2, 18)
ax = plt.subplot(111, projection='polar')
ax.plot(theta1, r1)
def step(self, action, return_obs=False):
self.count += 1
self.controller(action)
t = vrep.simxGetLastCmdTime(self.clientID)
vrep.simxSynchronousTrigger(self.clientID)
while vrep.simxGetLastCmdTime(self.clientID) - t < self.dt:
pose = vrep.simxGetObjectPosition(self.clientID, self.body_handle,
-1,
vrep.simx_opmode_streaming)[1]
orientation = vrep.simxGetObjectOrientation(
self.clientID, self.body_handle, -1,
vrep.simx_opmode_streaming)[1][2]
goal_pos = vrep.simxGetObjectPosition(
self.clientID, self.goal_handle, self.body_handle,
vrep.simx_opmode_streaming)[1][1:3]
lrf_bin = vrep.simxGetStringSignal(self.clientID, 'hokuyo_data',
vrep.simx_opmode_streaming)[1]
lrf = array(vrep.simxUnpackFloats(lrf_bin), dtype=float) / 5.578
goal_dist = linalg.norm(goal_pos)
goal_angle = arctan2(-goal_pos[0], goal_pos[1])
state1 = list(lrf) + [action[0] * 2, action[1]]
state1 += [goal_dist/5.578, goal_angle/pi] \
if goal_dist<5.578 else \
[1, goal_angle/pi]
if self.goal_dist_prev != None:
reward = self.reward(lrf, goal_dist, action)
self.goal_dist_prev = goal_dist
self.reward_sum += reward
sys.stderr.write(
'\rstep:%d| goal:% 2.1f, % 2.1f | pose:% 2.1f, % 2.1f | avg.reward:% 4.2f'
% (self.count, self.goal[0], self.goal[1], pose[0], pose[1],
self.reward_sum / self.count))
if min(lrf) < 0.0358:
done = 1
print(' | Fail')
elif goal_dist < 0.1:
done = 1
print(' | Success')
else:
done = 0
if self.state0 != None:
self.replay.add({
'state0': self.state0,
'action0': action,
'reward': reward,
'state1': state1,
'done': done
})
self.state0 = state1
self.action_prev = action
self.goal_dist_prev = goal_dist
if return_obs:
obs = {
'state': state1,
'lidar': list(lrf),
'action': action,
'pose': [pose[0], pose[1], orientation],
'goal': [goal_dist, goal_angle],
'reward': reward,
'time': t + self.dt
}
return state1, obs, done
else:
return state1, done