def set_resolution(self, resolution):
"""Set resolution."""
if self._handle < 0:
if self._handle == MISSING_HANDLE:
raise RuntimeError("Could not set resolution of {}: missing "
"name or handle.".format(self._name))
if self._handle == REMOVED_OBJ_HANDLE:
raise RuntimeError("Could not set resolution of {}: object "
"removed.".format(self._name))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not set resolution of {}: not connected to V-REP "
"remote API server.".format(self._name))
res = vrep.simxSetObjectIntParameter(
client_id, self._handle, vrep.sim_visionintparam_resolution_x,
resolution[0], vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not set resolution of {}."
"".format(self._name))
res = vrep.simxSetObjectIntParameter(
client_id, self._handle, vrep.sim_visionintparam_resolution_y,
resolution[1], vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not set resolution of {}."
"".format(self._name))
def lock_all_joints():
for i in range(0, len(joint_handles)):
vrep.simxSetJointTargetVelocity(clientID, joint_handles[i], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetObjectIntParameter(clientID, joint_handles[i],
vrep.sim_jointintparam_velocity_lock, 1,
vrep.simx_opmode_oneshot)
time.sleep(2)
def _read_vision_sensor(self, grayscale=False):
vrep.simxSetObjectIntParameter(
self.cid, self.vis_handle,
vrep.sim_visionintparam_entity_to_render,
self.rack_handle if grayscale else -1, vrep.simx_opmode_blocking)
_, _, _, byte_data = self.call_lua_function('get_image',
ints=[int(grayscale)])
num_channels = len(byte_data) // (self.res[0] * self.res[1])
return np.frombuffer(byte_data, dtype=np.uint8).reshape(
(self.res[0], self.res[1], num_channels))
def set_joint_vels(joint_act):
for i in range(0, len(joint_act)):
vrep.simxSetJointTargetVelocity(clientID, joint_handles[i],
joint_act[i], vrep.simx_opmode_oneshot)
vrep.simxSetObjectIntParameter(clientID, joint_handles[i],
vrep.sim_jointintparam_ctrl_enabled, 0,
vrep.simx_opmode_oneshot)
vrep.simxSetObjectIntParameter(clientID, joint_handles[i],
vrep.sim_jointintparam_velocity_lock, 1,
vrep.simx_opmode_oneshot)
def setObjectParameter(clientID,
handle,
parameter_code,
set_val,
is_float=True):
if is_float:
vrep.simxSetObjectFloatParameter(clientID, handle, parameter_code,
set_val, oneshot)
else:
vrep.simxSetObjectIntParameter(clientID, handle, parameter_code,
set_val, oneshot)
def move_back(vel_mb):
vrep.simxSetObjectIntParameter(clientID, joint_handles[3],
vrep.sim_jointintparam_ctrl_enabled, 0,
vrep.simx_opmode_oneshot)
print('Move back')
# move_back_count += 1
tic = time.clock()
vrep.simxSetJointTargetVelocity(clientID, joint_handles[3], -vel_mb * 2,
vrep.simx_opmode_oneshot)
time.sleep(3)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[3], 0,
vrep.simx_opmode_oneshot)
time.sleep(3)
return 2
def approach1(prop_thresh, vel_1, center_res, cent_weight, jps_thresh):
print('Approach1')
vrep.simxSetJointTargetVelocity(clientID, joint_handles[1], vel_1,
vrep.simx_opmode_oneshot)
vrep.simxSetObjectIntParameter(clientID, joint_handles[1],
vrep.sim_jointintparam_ctrl_enabled, 0,
vrep.simx_opmode_oneshot)
vrep.simxSetObjectIntParameter(clientID, joint_handles[1],
vrep.sim_jointintparam_velocity_lock, 1,
vrep.simx_opmode_oneshot)
if obj_x > resolution[0]/2 + center_res or obj_x < resolution[0]/2 - center_res or \
obj_y > resolution[1]/2 + center_res or obj_y < resolution[1]/2 - center_res:
center_object(cent_weight, obj_x, obj_y, resolution)
returncode, joint2_pos = vrep.simxGetJointPosition(clientID,
joint_handles[1],
vrep.simx_opmode_buffer)
returncode, joint5_pos = vrep.simxGetJointPosition(clientID,
joint_handles[4],
vrep.simx_opmode_buffer)
jointpossum = joint2_pos + joint5_pos
print(joint2_pos, joint5_pos)
# print(jointpossum)
if obj_pix_prop > prop_thresh or jointpossum > jps_thresh:
print('Finished Approach1!')
print('Object Pixel Proportion: ', obj_pix_prop)
print('Joint Position Sum: ', jointpossum)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[1], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0,
vrep.simx_opmode_oneshot)
return 2
# elif obj_pix_prop < 0.001:
# return 6
else:
return 1
def approach2(prop_thresh, vel_2, center_res, cent_weight):
# print("Approach2!")
vrep.simxSetJointTargetVelocity(clientID, joint_handles[3], vel_2,
vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], -vel_2/2, vrep.simx_opmode_oneshot)
vrep.simxSetObjectIntParameter(clientID, joint_handles[3],
vrep.sim_jointintparam_ctrl_enabled, 0,
vrep.simx_opmode_oneshot)
vrep.simxSetObjectIntParameter(clientID, joint_handles[3],
vrep.sim_jointintparam_velocity_lock, 1,
vrep.simx_opmode_oneshot)
if obj_x > resolution[0]/2 + center_res or obj_x < resolution[0]/2 - center_res or \
obj_y > resolution[1]/2 + center_res or obj_y < resolution[1]/2 - center_res:
center_object(cent_weight, obj_x, obj_y, resolution)
returncode, joint4_pos = vrep.simxGetJointPosition(clientID,
joint_handles[3],
vrep.simx_opmode_buffer)
returncode, joint5_pos = vrep.simxGetJointPosition(clientID,
joint_handles[4],
vrep.simx_opmode_buffer)
jointposdiff = joint4_pos - joint5_pos
# print(jointposdiff)
if obj_pix_prop > prop_thresh or jointposdiff > 3:
print('Finished Approach2!')
print('Object Pixel Proportion: ', obj_pix_prop)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[3], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0,
vrep.simx_opmode_oneshot)
return 3
elif obj_pix_prop < 0.001:
return 6
else:
return 2
def turn_EE():
global EE_turn
center_res = 10
vrep.simxSetObjectIntParameter(clientID, joint_handles[5],
vrep.sim_jointintparam_ctrl_enabled, 0,
vrep.simx_opmode_oneshot)
print('Turn EE!')
if EE_turn == 0:
EE_turn = 1
returncode, joint6_pos = vrep.simxGetJointPosition(
clientID, joint_handles[5], vrep.simx_opmode_buffer)
targetpos6 = joint6_pos + math.pi / 2
print('Target Angle: ', targetpos6)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[5], 0.1,
vrep.simx_opmode_oneshot)
while joint6_pos < targetpos6:
returncode, joint6_pos = vrep.simxGetJointPosition(
clientID, joint_handles[5], vrep.simx_opmode_buffer)
if joint6_pos >= targetpos6:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[5], 0,
vrep.simx_opmode_oneshot)
return 1
errorCode2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, cam_Handle, 0, vrep.simx_opmode_buffer)
im = np.array(image, dtype=np.uint8)
if len(resolution) > 0:
im.resize([resolution[0], resolution[1], 3])
[grasp_type, im_x, im_y,
obj_pix_prop] = image_process(im, resolution)
if not (((resolution[0]) / 2 - center_res < im_x <
(resolution[0]) / 2 + center_res) and
((resolution[1]) / 2 - center_res < im_y <
(resolution[1]) / 2 + center_res)):
center_object(.02, im_x, im_y, resolution)
def SetCameraOrientation(self, orientation):
# get resolution based on orientation
if orientation == 'portrait':
x_res = 480
y_res = 640
self.verticalViewAngle = self.robotParameters.cameraPerspectiveAngle
self.horizontalViewAngle = self.robotParameters.cameraPerspectiveAngle * x_res / y_res
elif orientation == 'landscape':
x_res = 640
y_res = 480
self.verticalViewAngle = self.robotParameters.cameraPerspectiveAngle * y_res / x_res
self.horizontalViewAngle = self.robotParameters.cameraPerspectiveAngle
else:
print('The camera orientation %s is not known. You must specify either portrait or landscape')
return
# update robot parameters
self.robotParameters.cameraOrientation = orientation
# set resolution of camera (vision sensor object) - resolution parameters are int32 parameters
vrep.simxSetObjectIntParameter(self.clientID, self.cameraHandle, vrep.sim_visionintparam_resolution_x, x_res, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectIntParameter(self.clientID, self.cameraHandle, vrep.sim_visionintparam_resolution_y, y_res, vrep.simx_opmode_oneshot_wait)
def vrep_change_properties(client_id, object_id, parameter_id,
parameter_value):
"""Changing properties of sensors in vrep
client_id (int): ID of current scene in vrep
object_id (int): ID of sensor to change
parameter_id (int): ID of parameter to change
parameter_value (int/double): value of parameter to change
"""
params_f = {
'near_clipping_plane': 1000,
'far_clipping_plane': 1001,
'perspective_angle': 1004
}
params_i = {
'vision_sensor_resolution_x': 1002,
'vision_sensor_resolution_y': 1003
}
if parameter_id == 'perspective_angle':
parameter_value = parameter_value / (180 * 2) * math.pi
if parameter_id in params_f:
error = vrep.simxSetObjectFloatParameter(client_id, object_id,
params_f[parameter_id],
parameter_value,
vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value,
vrep.simx_opmode_blocking)
vrep.simxClearFloatSignal(client_id, 'change_params',
vrep.simx_opmode_blocking)
return error
elif parameter_id in params_i:
error = vrep.simxSetObjectIntParameter(client_id, object_id,
params_i[parameter_id],
parameter_value,
vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value,
vrep.simx_opmode_blocking)
vrep.simxClearFloatSignal(client_id, 'change_params',
vrep.simx_opmode_blocking)
return error
else:
return 'parameter wasn\'t found'
def disableControlLoop():
for i in range(6):
vrep.simxSetObjectIntParameter(clientID, jointHandles[i],
vrep.sim_jointintparam_ctrl_enabled,
0, vrep.simx_opmode_blocking)
def search_object_bci(bci_iter,
this_bci_iter,
next_bci_iter,
clf,
num_channels=22):
time_limit = False
vrep.simxSetStringSignal(clientID, 'IKCommands', 'LookPos',
vrep.simx_opmode_oneshot) # "Looking" position
# vrep.simxSetObjectIntParameter(clientID, joint_handles[0], vrep.sim_jointintparam_velocity_lock, 0,
# vrep.simx_opmode_oneshot)
center_res = 10
bci_update = 0.25
tic_so = time.clock()
tic_bci = tic_so
objectFound = False
print('num_channels: ', num_channels)
bci_class, cert = real_time_BCI.get_bci_class(bci_iter,
clf,
num_channels=num_channels)
bci_iter = bci_iter + 1
if time_limit:
# while bci_time + 3 < tic_bci:
while bci_iter < this_bci_iter:
bci_class, cert = real_time_BCI.get_bci_class(bci_iter, clf)
bci_iter = bci_iter + 1
tic_bci = time.clock()
while objectFound is False:
toc_so = time.clock()
# print('time_diff: ', toc_so - tic_bci)
# print bci_iter, next_bci_iter
if bci_iter > next_bci_iter - 1:
return bci_iter, 6
if toc_so - tic_bci > bci_update:
bci_class, cert = real_time_BCI.get_bci_class(bci_iter, clf)
tic_bci = time.clock()
bci_iter = bci_iter + 1
# print bci_time, tic_bci
if bci_class == 1:
robot_joint_pub.publish()
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], -0.065,
vrep.simx_opmode_oneshot)
elif bci_class == 2:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0.065,
vrep.simx_opmode_oneshot)
elif bci_class == 0:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0,
vrep.simx_opmode_oneshot)
elif bci_class == 4:
vrep.simxSetObjectIntParameter(
clientID, joint_handles[0],
vrep.sim_jointintparam_velocity_lock, 1,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0,
vrep.simx_opmode_oneshot)
elif bci_class == 3:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0,
vrep.simx_opmode_oneshot)
objectFound = True
errorCode2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, cam_Handle, 0, vrep.simx_opmode_buffer)
im = np.array(image, dtype=np.uint8)
if len(resolution) > 0:
im.resize([resolution[0], resolution[1], 3])
image_process_2(im, resolution, bci_class)
return bci_iter, 1
def enableControlLoop(self):
for i in range(6):
vrep.simxSetObjectIntParameter(self.clientID, self.jointHandles[i],
vrep.sim_jointintparam_ctrl_enabled,
1, vrep.simx_opmode_blocking)
def __enter__(self):
print('[ROBOTENV] Starting environment...')
sync_mode_str = 'TRUE' if self.sync_mode else 'FALSE'
portNb_str = str(self.portNb)
# launch v-rep
vrep_cmd = [
self.vrepPath, '-gREMOTEAPISERVERSERVICE_' + portNb_str +
'_FALSE_' + sync_mode_str
]
if not self.showGUI:
vrep_cmd.append('-h') # headless mode
vrep_cmd.append(self.scenePath)
# headless mode via ssh
# vrep_cmd = "xvfb-run --auto-servernum --server-num=1 /homes/dam416/V-REP_PRO_EDU_V3_4_0_Linux/vrep.sh -h -s -q MicoRobot.ttt"
# vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', '-s', '-q', self.scenePath]
# vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', self.scenePath]
print('[ROBOTENV] Launching V-REP...')
# NOTE: do not use "stdout=subprocess.PIPE" below to buffer logs, causes deadlock at episode 464! (flushing the buffer may work... but buffering is not needed)
self.vrepProcess = subprocess.Popen(vrep_cmd,
shell=False,
preexec_fn=os.setsid)
# connect to V-Rep Remote Api Server
vrep.simxFinish(-1) # close all opened connections
# Connect to V-REP
print('[ROBOTENV] Connecting to V-REP...')
counter = 0
while True:
self.clientID = vrep.simxStart('127.0.0.1', self.portNb, True,
False, 5000, 0)
if self.clientID != -1:
break
else:
print("[ROBOTENV] Connection failed, retrying")
counter += 1
if counter == 10:
raise RuntimeError(
'[ROBOTENV] Connection to V-REP failed.')
if self.clientID == -1:
print('[ROBOTENV] Failed connecting to remote API server')
else:
print('[ROBOTENV] Connected to remote API server')
# close model browser and hierarchy window
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_browser_visible,
False, vrep.simx_opmode_blocking)
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_hierarchy_visible,
False, vrep.simx_opmode_blocking)
vrep.simxSetBooleanParameter(self.clientID,
vrep.sim_boolparam_console_visible,
False, vrep.simx_opmode_blocking)
# load scene
# time.sleep(5) # to avoid errors
# returnCode = vrep.simxLoadScene(self.clientID, self.scenePath, 1, vrep.simx_opmode_oneshot_wait) # vrep.simx_opmode_blocking is recommended
# # Start simulation
# # vrep.simxSetIntegerSignal(self.clientID, 'dummy', 1, vrep.simx_opmode_blocking)
# # time.sleep(5) #to center window for recordings
# if self.initial_joint_positions is not None:
# self.initializeJointPositions(self.initial_joint_positions)
# self.startSimulation()
# # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# # printlog('simxStartSimulation', returnCode)
# get handles and start streaming distance to goal
for i in range(0, self.nSJoints):
returnCode, self.jointHandles[i] = vrep.simxGetObjectHandle(
self.clientID, 'Mico_joint' + str(i + 1),
vrep.simx_opmode_blocking)
printlog('simxGetObjectHandle', returnCode)
returnCode, self.fingersH1 = vrep.simxGetObjectHandle(
self.clientID, 'MicoHand_fingers12_motor1',
vrep.simx_opmode_blocking)
returnCode, self.fingersH2 = vrep.simxGetObjectHandle(
self.clientID, 'MicoHand_fingers12_motor2',
vrep.simx_opmode_blocking)
returnCode, self.goalCubeH = vrep.simxGetObjectHandle(
self.clientID, 'GoalCube', vrep.simx_opmode_blocking)
returnCode, self.targetCubePositionH = vrep.simxGetObjectHandle(
self.clientID, 'GoalPosition', vrep.simx_opmode_blocking)
returnCode, self.robotBaseH = vrep.simxGetObjectHandle(
self.clientID, 'Mico_link1_visible', vrep.simx_opmode_blocking)
returnCode, self.jointsCollectionHandle = vrep.simxGetCollectionHandle(
self.clientID, "sixJoints#", vrep.simx_opmode_blocking)
returnCode, self.distToGoalHandle = vrep.simxGetDistanceHandle(
self.clientID, "distanceToGoal#", vrep.simx_opmode_blocking)
returnCode, self.endEffectorH = vrep.simxGetObjectHandle(
self.clientID, "DummyFinger#", vrep.simx_opmode_blocking)
# returnCode, self.distanceToGoal = vrep.simxReadDistance(self.clientID, self.distToGoalHandle, vrep.simx_opmode_streaming) #start streaming
# returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData(self.clientID, self.jointsCollectionHandle, 15, vrep.simx_opmode_streaming) #start streaming
if self.targetCubePosition is not None:
# hide goal position plane
visibility_layer_param_ID = 10
visible_layer = 1
returnCode = vrep.simxSetObjectIntParameter(
self.clientID, self.targetCubePositionH,
visibility_layer_param_ID, visible_layer,
vrep.simx_opmode_blocking)
# print('simxSetObjectIntParameter return Code: ', returnCode)
self.initializeGoalPosition()
# Start simulation
if self.initial_joint_positions is not None:
self.initializeJointPositions()
self.reset()
# self.startSimulation()
# # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# # printlog('simxStartSimulation', returnCode)
# self.updateState() # default initial state: 180 degrees (=pi radians) for all angles
# check the state is valid
# while True:
# self.updateState()
# print("\nstate: ", self.state)
# print("\nreward: ", self.reward)
# # wait for a state
# if (self.state.shape == (1,self.observation_space_size) and abs(self.reward) < 1E+5):
# print("sent reward3=", self.reward)
# break
print(
'[ROBOTENV] Environment succesfully initialised, ready for simulations'
)
# while vrep.simxGetConnectionId(self.clientID) != -1:
# ##########################EXECUTE ACTIONS AND UPDATE STATE HERE #################################
# time.sleep(0.1)
# #end of execution loop
return self
