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)
