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