def set_boolean_parameter(self, parameter_id, value): """ Sets a parameter of the simulation based on the parameter id """ vrep.simxSetBooleanParameter(self.client_id, parameter_id, value, vrep.simx_opmode_blocking)
def connect(self): """ Connect to the current scene open in VREP Finds the VREP references to the joints of the robot. Sets the time step for simulation and put into lock-step mode. NOTE: the joints and links must follow the naming convention of 'joint#' and 'link#', starting from 'joint0' and 'link0' NOTE: The dt in the VREP physics engine must also be specified to be less than the dt used here. """ # close any open connections vrep.simxFinish(-1) # Connect to the V-REP continuous server self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5) if self.clientID == -1: raise Exception('Failed connecting to remote API server') vrep.simxSynchronous(self.clientID, True) # get the handles for each joint and set up streaming self.joint_handles = [vrep.simxGetObjectHandle(self.clientID, name, vrep.simx_opmode_blocking)[1] for name in self.robot_config.JOINT_NAMES] # get handle for target and set up streaming _, self.misc_handles['target'] = \ vrep.simxGetObjectHandle(self.clientID, 'target', vrep.simx_opmode_blocking) # get handle for hand _, self.hand_handle = \ vrep.simxGetObjectHandle(self.clientID, 'hand', vrep.simx_opmode_blocking) vrep.simxSetFloatingParameter( self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt, # specify a simulation time step vrep.simx_opmode_oneshot) vrep.simxSetBooleanParameter( self.clientID, vrep.sim_boolparam_display_enabled, True, vrep.simx_opmode_oneshot) # start our simulation in lockstep with our code vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) print('Connected to VREP remote API server')
def reset(self): self.__stop() if self.IS_RECORD: self.__move() vrep.simxSetBooleanParameter( self.__ID, vrep.sim_boolparam_video_recording_triggered, True, vrep.simx_opmode_oneshot) vrep.simxSynchronous(self.__ID, True) vrep.simxStartSimulation(self.__ID, vrep.simx_opmode_blocking) self.__MODE.set(None) vrep.simxSynchronousTrigger(self.__ID) vrep.simxGetPingTime(self.__ID) # get initial states state, reward, done, info = self.__MODE.get() return state
def connect(self): print("Connecting to remote API server...", file=sys.stderr) lan_ip = socket.gethostbyname(socket.gethostname()) vrep.simxStopSimulation(-1, vrep.simx_opmode_oneshot_wait) vrep.simxFinish(-1) # just in case, close all opened connections self.clientID = vrep.simxStart(lan_ip, 19997, True, True, 5000, 5) assert self.clientID != -1, 'Failed to connect to the V-rep server' # Register all handles print("Registering all handle of scene... (Rendering = {})".format( self.render), file=sys.stderr) # if scene modified, Call this function to update handle list < DEBUG MODE !!!> #_chk_handle_list() vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait) # Is Render? err = vrep.simxSetBooleanParameter(self.clientID, vrep.sim_boolparam_display_enabled, self.render, vrep.simx_opmode_oneshot) # Data streaming self._data_streaming() print(datetime.datetime.now(), " Complete registered. Simulation ready", file=sys.stderr)
def startSim(self, clientID, screen=True): #I need the simulator stopped in order to be started retSimStop = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) if retSimStop != simx_return_ok : print "simulation couldnt be stopped!" else: print "simulation stopped!" #setting the physics engine retSetPhyEngine = vrep.simxSetIntegerParameter(clientID, vrep.sim_intparam_dynamic_engine, bulletEngine, vrep.simx_opmode_oneshot_wait) if retSetPhyEngine != simx_return_ok: print "unable to set the physical engine" else: print "physical engine correctly setted" if int(self.sysConf.getProperty("physics enable?"))==1: vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,True,vrep.simx_opmode_oneshot) else: vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,False,vrep.simx_opmode_oneshot) #settig simulation speed if self.speedmodifier > 0: vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait) #settig simulation step retSetTimeStep = vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, self.simulationTimeStepDT, vrep.simx_opmode_oneshot_wait) if retSetTimeStep != simx_return_ok : print "problems setting time step" else: print "time step setted!" #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_realtime_simulation,1,vrep.simx_opmode_oneshot_wait) #sync mode configuration if self.synchronous: vrep.simxSynchronous(clientID,True) #light mode configuration if not screen: vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_visible_layers,2,vrep.simx_opmode_oneshot_wait) #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait) #start simulation error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot) return error
def start_new_episode(self, episode): # Stop the simulation if it is running vrep.simxStopSimulation(self.__client_ID, vrep.simx_opmode_blocking) time.sleep(1) # One second delay # Start simulation vrep.simxStartSimulation(self.__client_ID, vrep.simx_opmode_blocking) # Set display to enabled or not vrep.simxSetBooleanParameter(self.__client_ID, vrep.sim_boolparam_display_enabled, self.__show_display, vrep.simx_opmode_oneshot) # Enable threaded rendering vrep.simxSetBooleanParameter(self.__client_ID, vrep.sim_boolparam_threaded_rendering_enabled, 1, vrep.simx_opmode_oneshot) # Catch error error = vrep.simxSynchronous(self.__client_ID, 1) assert error == 0, "Could not start simulation in synchronous mode." # Reset all agents for agent in self.__agents: agent.restart()
def reset(self): self.__stop() self.state = np.zeros_like(self.state) self.reward = 0.0 self.done = False self.action = np.zeros_like(self.action) if self.IS_RECORD: self.__move() vrep.simxSetBooleanParameter( self.__ID, vrep.sim_boolparam_video_recording_triggered, True, vrep.simx_opmode_oneshot) vrep.simxSynchronous(self.__ID, True) vrep.simxStartSimulation(self.__ID, vrep.simx_opmode_blocking) self.__set(self.action) vrep.simxSynchronousTrigger(self.__ID) # get initial states self.__get() return self.state
def rendering(True_or_False): if (not clientID): print('connect first') elif (clientID[0] == -1): print('connect first') else: err = vrep.simxSetBooleanParameter(clientID[0], vrep.sim_boolparam_display_enabled, True_or_False, vrep.simx_opmode_oneshot)
def display_disabled(self): vrep.simxSetBooleanParameter(self.clientID, vrep.sim_boolparam_display_enabled, False, vrep.simx_opmode_oneshot_wait) vrep.simxSetBooleanParameter(self.clientID, vrep.sim_boolparam_browser_visible, False, vrep.simx_opmode_oneshot_wait) vrep.simxSetBooleanParameter(self.clientID, vrep.sim_boolparam_hierarchy_visible, False, vrep.simx_opmode_oneshot_wait) return
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
def __init__(self, scene="rollbalance", is_render=True, is_boot=True): # import V-REP if "linux" in sys.platform: self.VREP_DIR = os.path.expanduser("~") + "/V-REP_PRO_EDU/" vrepExe = "vrep.sh" elif "darwin" in sys.platform: self.VREP_DIR = "/Applications/V-REP_PRO_EDU/" vrepExe = "vrep.app/Contents/MacOS/vrep" else: print(sys.platform) sys.stderr.write("I don't know how to use vrep in Windows...\n") sys.exit(-1) sys.path.append(self.VREP_DIR + "programming/remoteApiBindings/python/python/") try: global vrep import vrep except: print( '--------------------------------------------------------------' ) print( '"vrep.py" could not be imported. This means very probably that' ) print( 'either "vrep.py" or the remoteApi library could not be found.' ) print('Make sure both are in the same folder as this file,') print('or appropriately adjust the file "vrep.py"') print( '--------------------------------------------------------------' ) sys.exit(-1) # start V-REP self.IS_BOOT = is_boot if self.IS_BOOT: vrepArgs = [ self.VREP_DIR + vrepExe, os.path.abspath(os.path.dirname(__file__)) + "/scenes/" + scene + ".ttt" ] if not is_render: vrepArgs.extend(["-h"]) vrepArgs.extend(["&"]) self.vrepProcess = subprocess.Popen(vrepArgs, stdout=open(os.devnull, 'w'), stderr=subprocess.STDOUT, preexec_fn=os.setsid) print("Enviornment was opened: {}, {}".format( vrepArgs[0], vrepArgs[1])) # connect to V-REP ipAddress = "127.0.0.1" portNum = 19997 self.__ID = vrep.simxStart(ipAddress, portNum, True, True, 5000, 1) while self.__ID == -1: self.__ID = vrep.simxStart(ipAddress, portNum, True, True, 5000, 1) # start to set constants vrep.simxSynchronous(self.__ID, True) vrep.simxStartSimulation(self.__ID, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.__ID) if is_render: vrep.simxSetBooleanParameter(self.__ID, vrep.sim_boolparam_display_enabled, False, vrep.simx_opmode_oneshot) # constant shared with lua self.DT = vrep.simxGetFloatSignal(self.__ID, "dt", vrep.simx_opmode_blocking)[1] max_state = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.__ID, "max_state", vrep.simx_opmode_blocking)[1])) max_action = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.__ID, "max_action", vrep.simx_opmode_blocking)[1])) min_state = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.__ID, "min_state", vrep.simx_opmode_blocking)[1])) min_action = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.__ID, "min_action", vrep.simx_opmode_blocking)[1])) # limits of respective states and action self.observation_space = spaces.Box(min_state, max_state) self.action_space = spaces.Box(min_action, max_action) # variables will be received self.state = np.zeros(len(max_state)) self.reward = 0.0 self.done = False # variables will be sended self.action = np.zeros(len(max_action)) # enable streaming self.state = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.__ID, "states", vrep.simx_opmode_streaming)[1])) self.reward = vrep.simxGetFloatSignal(self.__ID, "reward", vrep.simx_opmode_streaming)[1] self.done = bool( vrep.simxGetIntegerSignal(self.__ID, "done", vrep.simx_opmode_streaming)[1]) # stop simulation self.__stop() self.IS_RECORD = False
def startSim(clientID, screen=True): error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) if not screen: vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait) return error
def __init__(self, scene="rollbalance", is_render=True, is_boot=True, port=19997): # import V-REP if "linux" in sys.platform: self.VREP_DIR = os.path.expanduser("~") + "/V-REP_PRO_EDU/" exeDir = "" vrepExe = "vrep.sh" libExe = "Linux/64Bit/remoteApi.so" elif "darwin" in sys.platform: self.VREP_DIR = "/Applications/V-REP_PRO_EDU/" exeDir = "vrep.app/Contents/MacOS/" vrepExe = "vrep" libExe = "Mac/remoteApi.dylib" else: print(sys.platform) sys.stderr.write("I don't know how to use vrep in Windows...\n") sys.exit(-1) # copy remote api library to the accesible directory remotePath = self.VREP_DIR + "programming/remoteApiBindings/" sys.path.append(remotePath + "python/python/") try: remoteFile = remotePath + "python/python" + libExe[libExe.rfind("/" ):] if not os.path.exists(remoteFile): print(remoteFile + "is not found, so copied from lib directory.") shutil.copyfile(remotePath + "lib/lib/" + libExe, remoteFile) except: pass # load remote api library try: global vrep import vrep except: print( '--------------------------------------------------------------' ) print( '"vrep.py" could not be imported. This means very probably that' ) print( 'either "vrep.py" or the remoteApi library could not be found.' ) print('Make sure both are in the same folder as this file,') print('or appropriately adjust the file "vrep.py"') print( '--------------------------------------------------------------' ) sys.exit(-1) # confirm scene file and change mode depending on the scene # scenePath = os.path.abspath(os.path.dirname(__file__))+"/scenes/"+scene+".ttt" scenePath = os.path.abspath(os.path.dirname(__file__)) + "/scenes/" modeName = "normal" for d in [ x for x in os.listdir(scenePath) if os.path.isdir(scenePath + x) ]: if scene + ".ttt" in os.listdir(scenePath + d): modeName = d scenePath += d + "/" + scene + ".ttt" break # start V-REP with fasteners.InterProcessLock( os.path.abspath(os.path.dirname(__file__)) + ".lockfile"): self.IS_BOOT = is_boot if self.IS_BOOT: # change port number content = "" with open(self.VREP_DIR + exeDir + "remoteApiConnections.txt", "r") as f_handle: content = f_handle.read().splitlines() target = content[11].split("=") target[1] = " " + str(port) content[11] = "=".join(target) with open(self.VREP_DIR + exeDir + "remoteApiConnections.txt", "w") as f_handle: f_handle.write("\n".join(content)) # open vrep vrepArgs = [self.VREP_DIR + exeDir + vrepExe, scenePath] if not is_render: vrepArgs.extend(["-h"]) vrepArgs.extend(["&"]) self.vrepProcess = subprocess.Popen(vrepArgs, stdout=open( os.devnull, 'w'), stderr=subprocess.STDOUT, preexec_fn=os.setsid) print("Enviornment was opened:\n{}\n{}".format( vrepArgs[0], scenePath)) else: # get port number (assume that the opened vrep loaded the current text) content = "" with open(self.VREP_DIR + exeDir + "remoteApiConnections.txt", "r") as f_handle: content = f_handle.read().splitlines() target = content[11].split("=") port = int(target[1]) # connect to V-REP ipAddress = "127.0.0.1" self.__ID = vrep.simxStart(ipAddress, port, True, True, 5000, 1) while self.__ID == -1: self.__ID = vrep.simxStart(ipAddress, port, True, True, 5000, 1) print("Connection succeeded: {}:{}".format(ipAddress, port)) # open scene if already booted if not self.IS_BOOT: vrep.simxLoadScene(self.__ID, scenePath, 0, vrep.simx_opmode_blocking) print("Scene was opened:\n{}".format(scenePath)) # start to set constants vrep.simxSynchronous(self.__ID, True) vrep.simxStartSimulation(self.__ID, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.__ID) if is_render: vrep.simxSetBooleanParameter(self.__ID, vrep.sim_boolparam_display_enabled, False, vrep.simx_opmode_oneshot) # set functions for corresponding modes if "normal" in modeName: self.__MODE = ModeN(self.__ID) self.DT, self.observation_space, self.action_space = self.__MODE.define( ) elif "multi_objective" in modeName: self.__MODE = ModeMO(self.__ID) self.DT, self.observation_space, self.action_space, self.TASK_NUM = self.__MODE.define( ) elif "multi_agent" in modeName: self.__MODE = ModeMA(self.__ID) self.DT, self.observation_space, self.action_space, self.AGE_NUM = self.__MODE.define( ) # stop simulation self.__stop() self.IS_RECORD = False
def enable_display(self, paramValue): self.RAPI_rc( vrep.simxSetBooleanParameter(self.cID, vrep.sim_boolparam_display_enabled, paramValue, vrep.simx_opmode_blocking))
robotTree = etree.fromstring(robotXmlString) #机械臂本体 robotJointNames = [node.attrib['name'] for node in robotTree.xpath("//joint")] robotLinkNames = armLinkNames + handLinkNames jointPoses = parseUrdfJoint(urdfTree, robotJointNames, robotTree) vrep.simxFinish(-1) # 关闭所有连接 clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # 连接到VREP if clientID != -1: print('Connected to remote API server') # 关闭动力学 vrep.simxSetBooleanParameter(clientID, vrep.sim_boolparam_dynamics_handling_enabled, False, vrep.simx_opmode_blocking) virtualJHandles = [] for jname, jpose in jointPoses.items(): translate, angles = jpose res, dummy = vrep.simxCreateDummy(clientID, 0.05, None, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, dummy, -1, translate, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(clientID, dummy, -1, angles, vrep.simx_opmode_blocking) virtualJHandles.append(dummy) # 1.显示消息并返回反馈 emptyBuff = bytearray()
def set_boolean_parameter(self, param_id, param_val): return self.RAPI_rc(vrep.simxSetBooleanParameter( self.cID, param_id, param_val, vrep.simx_opmode_blocking))
def enable_threaded_rendering(self, paramValue): self.RAPI_rc(vrep.simxSetBooleanParameter( self.cID, vrep.sim_boolparam_threaded_rendering_enabled, paramValue, vrep.simx_opmode_blocking), tolerance=vrep.simx_return_remote_error_flag)