def _get_state(self):
state = {}
for key in self.robotS.active_joint_keys:
state['rS_' + key] = round(vp.getJointPosition(self.clientID, self.robotS.joint_dict[key]), self.precision)
for key in self.robotE.active_joint_keys:
state['rE_' + key] = round(vp.getJointPosition(self.clientID, self.robotE.joint_dict[key]), self.precision)
obj_pos = vp.getAbsolutePosition(self.clientID, self.object_handle)
for i in range(len(obj_pos)):
obj_pos[i] = round(obj_pos[i], self.precision)
state['obj_x'], state['obj_y'], state['obj_z'] = obj_pos
return state
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('----------------------------------------------')
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)