def stop_simulation(self): vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking) # stop simulation _, sim_state = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state ) # retrieve current sim play-pause-stop state while sim_state != 0: vrep.simxStopSimulation( self.clientID, vrep.simx_opmode_blocking) # stop simulation _, sim_state = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state ) # retrieve current sim play-pause-stop state
def start_simulation(self): vrep.simxSynchronous( self.clientID, True) # Enable the synchronous mode (Blocking function call) vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) # start simulation _, sim_state = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state ) # retrieve current sim play-pause-stop state while sim_state != 1: vrep.simxStartSimulation( self.clientID, vrep.simx_opmode_blocking) # stop simulation _, sim_state = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state ) # retrieve current sim play-pause-stop state
def reset(self): vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking) sensorDistance = self.reinitialize_robot() # vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_blocking) # ret_val = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) message = 0 count = 0 while ((message & 1) == 0): count += 1 print("SIMULATION STOPPED") result, message = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state) ret_val = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait) print("message = ", message) # if count != 3: # print("reset") # return count print("SIMULATION STARTED ", count) start_state, done = self.get_observation(sensorDistance) return start_state
def stop_if_needed(self): try_count = 0 while True: try_count += 1 vrep.simxSetIntegerSignal(self.clientID, 'dummySignal', 1, vrep.simx_opmode_blocking) # returnCode, ping = vrep.simxGetPingTime(self.clientID) returnCode, serverState = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state) # printlog('\nsimxGetInMessageInfo', returnCode) # print('\nServer state: ', serverState) # NOTE: if synchronous mode is needed: # check http://www.forum.coppeliarobotics.com/viewtopic.php?f=5&t=6603&sid=7939343e5e04b699af2d46f8d6efe7ba stopped = not (serverState & 1) if stopped: if try_count == 1: print("[ROBOTENV] Simulation is already stopped.") else: print("[ROBOTENV] Simulation stopped.") break else: if try_count == 1: print('[ROBOTENV] Stopping simulation...') returnCode = vrep.simxStopSimulation( self.clientID, vrep.simx_opmode_blocking) if returnCode != vrep.simx_return_ok: print( "[ROBOTENV] simxStopSimulation failed, error code:", returnCode)
def stop_simulation(self): if not self.sim_running: raise RuntimeError('Simulation is not running.') self.RAPI_rc(vrep.simxStopSimulation(self.cID, vrep.simx_opmode_blocking)) # Checking if the server really stopped try: while True: self.RAPI_rc(vrep.simxGetIntegerSignal(self.cID,'sig_debug',vrep.simx_opmode_blocking)) e = vrep.simxGetInMessageInfo(self.cID,vrep.simx_headeroffset_server_state) still_running = e[1] & 1 if not still_running: break except: pass self.sim_running = False
def reset(self): vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking) sensorDistance = self.reinitialize_robot() message = 0 while ((message & 1) == 0): print("SIMULATION STOPPED") ret_val = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) result, message = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state) print("SIMULATION STARTED") start_state, done = self.get_observation(sensorDistance) return start_state
def __init__(self): rospy.init_node('Dummy', anonymous=True) rospy.Subscriber("/restart", Int8, self.receiveStatus, queue_size=1) fin = rospy.Publisher('/finished', Int8, queue_size=1) vrep.simxFinish(-1) #clean up the previous stuff clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: print("Could not connect to server") sys.exit() first = True counter = 0 while (counter < episodes): print("Episode Number ", counter + 1) r = 1 if not first: time.sleep(.5) while r != 0: r = vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) start = time.time() self.restart = False elapsed = 0 while (not self.restart and elapsed < maxTime): curr = time.time() elapsed = curr - start vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking) if not self.restart: #timed out! msg = Int8() msg.data = 2 fin.publish(msg) print(' #### Timed out!') is_running = True while is_running: error_code, ping_time = vrep.simxGetPingTime(clientID) error_code, server_state = vrep.simxGetInMessageInfo( clientID, vrep.simx_headeroffset_server_state) is_running = server_state & 1 counter += 1 first = False time.sleep(2) msg = Int8() msg.data = 1 fin.publish(msg)
def is_sim_started(self): """Retrieve whether V-REP simulation is started. The return value may be inaccurate if the function is called immediately after starting or stopping a simulation; in such a case, introducing a short delay before calling it should help. """ SIM_NOT_STOPPED = 0x01 # Retrieve whether V-REP is currently waiting for a trigger signal; # the result by itself, however, is not conclusive as to whether a # simulation is started or not (not waiting for a trigger signal does # not necessarily mean that a simulation is not started because there # may be unprocessed trigger signals during a simulation, in # which case V-REP will be advancing the simulation without reporting # that it needs to wait for a trigger signal); this operation is # performed only to receive a new message from the V-REP remote API # server so that the next operation could operate on up-to-date data if self._client_id is None: raise ConnectionError( "Could not retrieve whether V-REP simulation is started: not " "connected to V-REP remote API server.") res, _ = vrep.simxGetBooleanParameter( self._client_id, vrep.sim_boolparam_waiting_for_trigger, vrep.simx_opmode_blocking) if res != vrep.simx_return_ok: raise ServerError("Could not retrieve whether V-REP simulation is " "started.") # Retrieve the server state from the last message received from the # V-REP remote API server res, server_state = vrep.simxGetInMessageInfo( self._client_id, vrep.simx_headeroffset_server_state) if res == -1: raise ServerError("Could not retrieve whether V-REP simulation is " "started.") # Determine whether V-REP simulation is started return bool(server_state & SIM_NOT_STOPPED)
returnCode = vrep.simxSetJointTargetVelocity( clientID, jointHandles[0], -vel, vrep.simx_opmode_blocking) printlog('simxSetJointTargetVelocity', returnCode) elif c == 'o': returnCode = vrep.simxSetJointTargetVelocity( clientID, jointHandles[0], 0, vrep.simx_opmode_blocking) printlog('simxSetJointTargetVelocity', returnCode) elif c == 'z': # stop simulation returnCode = vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking) printlog('simxStopSimulation', returnCode) running = True while running: returnCode, ping = vrep.simxGetPingTime(clientID) returnCode, serverState = vrep.simxGetInMessageInfo( clientID, vrep.simx_headeroffset_server_state) running = serverState # Start simulation returnCode = vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) printlog('simxStartSimulation', returnCode) elif c == 'q': break #testing getState returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData( clientID, jointsCollectionHandle, 15, vrep.simx_opmode_streaming ) # or simx_opmode_blocking (not recommended) jointPositions = np.array( floatData[0::2]
def check_simulation_state(self): self.wait_for_ping() return vrep.unwrap_vrep(vrep.simxGetInMessageInfo( self._clientID, vrep.simx_headeroffset_server_state), ignore_novalue_error=True)
def __stop(self): vrep.simxSynchronous(self.__ID, False) vrep.simxStopSimulation(self.__ID, vrep.simx_opmode_blocking) while vrep.simxGetInMessageInfo( self.__ID, vrep.simx_headeroffset_server_state)[1] % 2 == 1: pass
vrep.simxSetIntegerSignal( clientID,'asdf',1,vrep.simx_opmode_blocking) for j in range(5): print('---------------------simulation',j) # IMPORTANT # you should poll the server state to make sure # the simulation completely stops before starting a new one while True: # poll the useless signal (to receive a message from server) vrep.simxGetIntegerSignal( clientID,'asdf',vrep.simx_opmode_blocking) # check server state (within the received message) e = vrep.simxGetInMessageInfo(clientID, vrep.simx_headeroffset_server_state) # check bit0 not_stopped = e[1] & 1 if not not_stopped: break else: print('not_stopped') # IMPORTANT # you should always call simxSynchronous() # before simxStartSimulation() vrep.simxSynchronous(clientID,True) # then start the simulation: e = vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)
# Update running average of the reward if j >= options.RUNNING_AVG_STEP: avg_epi_reward_data[j] = np.mean( reward_data[j - options.RUNNING_AVG_STEP + 1:j]) # Stop and Restart Simulation Every X episodes if j % options.RESET_EPISODE == 0: print("Resetting...") vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking) # Wait until simulation is stopped. simulation_status = 1 bit_mask = 1 while bit_mask & simulation_status != 0: # Get right-most bit and check if it is 1 _, simulation_status = vrep.simxGetInMessageInfo( clientID, vrep.simx_headeroffset_server_state) # print(bit_mask & simulation_status) # print("{0:b}".format(simulation_status)) time.sleep(0.1) # Start simulation and initilize scene vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) time.sleep(1.0) setMotorPosition(clientID, steer_handle, 0) setMotorSpeed(clientID, motor_handle, 5) # save progress every 1000 episodes AND testing is disabled if options.TESTING == False: if j // options.SAVER_RATE >= 1 and j % options.SAVER_RATE == 0 and options.NO_SAVE == False: print("Saving network...") saver.save(
def _reset(self): if self._firstSim: self._connect() vrep.simxLoadScene(self.clientID, self._scene, 0, vrep.simx_opmode_blocking) else: vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot) number_of_iteration_not_stopped = 0 while True: vrep.simxGetIntegerSignal(self.clientID, 'asdf', vrep.simx_opmode_blocking) e = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state) not_stopped = e[1] & 1 if not not_stopped: print("STOPPED") break else: if number_of_iteration_not_stopped % 10 == 0: print(number_of_iteration_not_stopped, ': not stopped') number_of_iteration_not_stopped += 1 if number_of_iteration_not_stopped > 100: self._firstSim = True self._connect() vrep.simxLoadScene(self.clientID, self._scene, 0, vrep.simx_opmode_blocking) errorCode, handles, intData, floatData, stringData = vrep.simxGetObjectGroupData( self.clientID, vrep.sim_appobj_object_type, 0, vrep.simx_opmode_blocking) self._handles_dict = dict(zip(stringData, handles)) tmp = [] self.new_trajectory = {key: list(tmp) for key in self._order} for el in self._order: vrep.simxSetJointTargetPosition(self.clientID, self._handles_dict[el], 0, vrep.simx_opmode_oneshot) vrep.simxGetJointPosition(self.clientID, self._handles_dict[el], vrep.simx_opmode_streaming) # vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot) for i in range(self._range): if self._firstSim: print("FirStSim iteration: ", i) for el in self._order: vrep.simxSynchronousTrigger(self.clientID) vrep.simxSetJointTargetPosition( self.clientID, self._handles_dict[el], self._rad(self._walking_data[el][i]), vrep.simx_opmode_streaming) else: print("Reset iteration: ", i) for el in self._order: vrep.simxSynchronousTrigger(self.clientID) vrep.simxSetJointTargetPosition( self.clientID, self._handles_dict[el], self._rad(self._walking_data[el][i]), vrep.simx_opmode_streaming) if i == (self._range - 2): lastposlist = [] for jo in self._order: errorCode, lastpostmp = vrep.simxGetJointPosition( self.clientID, self._handles_dict[jo], vrep.simx_opmode_streaming) lastposlist.append(lastpostmp) rightSenValues, leftSenValues = _GetSensorValues( self.clientID, self._rightSen, self._leftSen, self._handles_dict) # print("RightSensors: ", rightSenValues) # print("LeftSensors: ", leftSenValues) rightSenSum = np.sum(np.asarray(rightSenValues)) leftSenSum = np.sum(np.asarray(leftSenValues)) zmpX, zmpY = _GetZmp(self.clientID, self._handles_dict, self._floor, rightSenValues, leftSenValues, rightSenSum, leftSenSum) # print("ZMPX: ", zmpX) # print("ZMPy: ", zmpY) errorCode, sphere = vrep.simxGetObjectPosition( self.clientID, self._handles_dict['Sphere'], self._handles_dict[self._floor], vrep.simx_opmode_streaming) self._sphere_height = sphere[2] # print("Sphere height: ", self._sphere_height) CurrentPos, CurrentVel = _GetJointValues(self.clientID, self._handles_dict, self._order, lastposlist) # print("Position: ", CurrentPos) # print("Velocity: ", CurrentVel) self._state.Reset() self._state.SetPosVel(CurrentPos, CurrentVel) if self._firstSim: self._firstSim = False self._range = 15 self._reset() self._done = False self._quit = False return self._state.GetState()
# setup a useless signal vrep.simxSetIntegerSignal(clientID, 'asdf', 1, vrep.simx_opmode_blocking) for j in range(5): print('---------------------simulation', j) # IMPORTANT # you should poll the server state to make sure # the simulation completely stops before starting a new one while True: # poll the useless signal (to receive a message from server) vrep.simxGetIntegerSignal(clientID, 'asdf', vrep.simx_opmode_blocking) # check server state (within the received message) e = vrep.simxGetInMessageInfo(clientID, vrep.simx_headeroffset_server_state) # check bit0 not_stopped = e[1] & 1 if not not_stopped: break else: print('not_stopped') # IMPORTANT # you should always call simxSynchronous() # before simxStartSimulation() vrep.simxSynchronous(clientID, True) # then start the simulation: e = vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
def __init__(self): rospy.init_node('Dummy', anonymous=True) rospy.Subscriber("/restart", Int8, self.receiveStatus, queue_size=1) fin = rospy.Publisher('/finished', Int8, queue_size=1) report_sim = rospy.Publisher('/simulation', String, queue_size=1) starting = rospy.Publisher('/starting', Int16, queue_size=1) vrep.simxFinish(-1) #clean up the previous stuff clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: print("Could not connect to server") sys.exit() first = True counter = 0 while (counter < episodes): print("Episode Number ", counter + 1) r = 1 if True: #not first: if (counter) % 50 == 0 and counter != 0: print('Sleep for 60') #time.sleep(60) else: time.sleep(3) simulation_index = np.random.randint(len(VREP_SCENES)) sim_name, sim_path = VREP_SCENES[simulation_index] msg = String() msg.data = sim_name report_sim.publish(msg) vrep.simxLoadScene(clientID, sim_path, 0, vrep.simx_opmode_blocking) time.sleep(2) while r != 0: r = vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) msg = Int16() msg.data = counter + 1 starting.publish(msg) start = time.time() self.restart = False elapsed = 0 while (not self.restart and elapsed < maxTime): curr = time.time() elapsed = curr - start vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot) if not self.restart: #timed out! msg = Int8() msg.data = 2 fin.publish(msg) print(' #### Timed out!') is_running = True while is_running: error_code, ping_time = vrep.simxGetPingTime(clientID) error_code, server_state = vrep.simxGetInMessageInfo( clientID, vrep.simx_headeroffset_server_state) is_running = server_state & 1 counter += 1 first = False time.sleep(2) msg = Int8() msg.data = 1 fin.publish(msg)