def _take_action(self, action):
tag = int(np.floor((action - 1) / 2))
increase = True if action % 2 == 1 else False
if tag == -1:
prefix = joint_key = 0
set_val = get_val = 0
joint_handle = None
elif tag >= 0 and tag < self.robotS.num_active_joints:
prefix = 'rS_'
joint_key = self.robotS.active_joint_keys[tag]
joint_handle = self.robotS.joint_dict[joint_key]
elif tag >= 0 and tag < self.total_active_joints:
prefix = 'rE_'
tag = tag - self.robotS.num_active_joints
joint_key = self.robotE.active_joint_keys[tag]
joint_handle = self.robotE.joint_dict[joint_key]
if prefix + joint_key != 0:
curr_val = self.state_dict[prefix + joint_key]
if joint_key == 'gripAction':
set_val = self.maxGripOpen if increase else 0.0
if prefix == 'rS_':
self.robotS.gripClosed = True if set_val == 0 else False
elif prefix == 'rE_':
self.robotE.gripClosed = True if set_val == 0 else False
else:
set_val = curr_val + self.dtheta if increase else curr_val - self.dtheta
vp.setJointPosition(self.clientID, joint_handle, set_val)
vp.syncSpinOnce(self.clientID)
vp.performBlockingOp(self.clientID)
if self.debugMode:
get_val = vp.getJointPosition(
self.clientID, joint_handle) if joint_handle != None else 0
gripS = vp.getJointForce(self.clientID,
self.robotS.joint_dict['gripAction'])
gripE = vp.getJointForce(self.clientID,
self.robotE.joint_dict['gripAction'])
print('----------------------------------------------')
print('Action taken = {}'.format(action))
print('Tag = {}, Increase = {}, State Key = {}'.format(
tag, increase, prefix + joint_key))
print('SetVal = {}, GetVal = {}'.format(
round(vp.r2d(set_val), self.precision),
round(vp.r2d(get_val), self.precision)))
print('GripS Force = {}, GripE Force = {}'.format(
round(gripS, self.precision), round(gripE, self.precision)))
print('Robots holding object = {}'.format(self._object_in_grip()))
print('----------------------------------------------')
def perform_episode(e):
clientID, robotS, robotE, env = setup_env()
vp.setSimulationTimeStep(clientID, dt)
vp.startSimulation(clientID)
vp.syncSpinOnce(clientID)
vp.performBlockingOp(clientID)
state = env._get_state()
state = env._convert_dict_to_tuple()
state = np.reshape(state, [1, state_size])
net_epi_score = 0
all_epi_scores = []
for step in range(MAX_STEPS):
action1, action2 = agent.choose_action2(state)
next_state, reward, done = env.step(action1)
net_epi_score += reward
all_epi_scores.append((net_epi_score, reward))
next_state = np.reshape(next_state, [1, state_size])
agent.remember_experience(state, action1, reward, next_state, done)
state = next_state
if done and step != 0:
print(agent.model.predict(state)[0])
print(action1)
break
next_state, reward, done = env.step(action2)
net_epi_score += reward
all_epi_scores.append((net_epi_score, reward))
next_state = np.reshape(next_state, [1, state_size])
agent.remember_experience(state, action2, reward, next_state, done)
state = next_state
if done and step != 0:
print(agent.model.predict(state)[0])
print(action2)
break
vp.stopSimulation(clientID)
vp.closeConnection(clientID)
return net_epi_score, all_epi_scores, step
vp.loadVREPScene(clientID, path_to_scene)
pxp_base_handle = vp.loadModelIntoScene(clientID, path_to_pxp, [0, -0.5, None],
[90, 0, 90])
joint_handles = vp.getAllJointHandles(clientID, joint_names)
vp.setSimulationTimeStep(clientID, dt)
vp.startSimulation(clientID)
#%% Simulate
while sim_time < stop_sim_time:
print 't =', sim_time
joint_handle = joint_handles[1]
get_joint_val = vp.getJointPosition(clientID, joint_handle)
vp.setJointPosition(clientID, joint_handle, set_joint_val)
sim_time += dt
set_joint_val += dval
print get_joint_val
vp.syncSpinOnce(clientID)
#%% Close
vp.stopSimulation(clientID)
vp.closeConnection(clientID)