def recordThrow(ballID, i):
base = getBaseLink()
vrep.simxSynchronous(clientID, True)
ballPos = getBallPos(ballID, base)
ballHeights = [ballPos[2]] # * ball_min_window
throwBall(i)
vrep.simxSynchronousTrigger(clientID)
j = 0
while True:
# check if ballPos z is lower than minimum
ballPos = getBallPos(ballID, -1) # base
ballHeights.append(ballPos[2])
# if yes, exist, return max height
# else simxSynchronousTrigger(clientID)
vrep.simxSynchronousTrigger(clientID)
j += 1
if j > 5:
if ballPos[2] <= z_ball_threshold or j == 50:
break
vrep.simxSynchronous(clientID, False)
return max(ballHeights)
def __init__(self):
self.portNumb = 19997
self.actions = []
self.score = 0
self.hasfallen = False
self.LUMSpeed = 0
self.LLMSpeed = 0
self.RUMSpeed = 0
self.RLMSpeed = 0
self.clientID = vrep.simxStart('127.0.0.1', self.portNumb, True, True,
5000, 5)
if self.clientID != -1:
print("Se pudo establecer la conexión con la api del simulador")
LUM, LLM, RUM, RLM, head = recoverRobotParts(self.clientID)
self.LUM = LUM
self.LLM = LLM
self.RUM = RUM
self.RLM = RLM
self.head = head
#Set simulation to be Synchonous instead of Asynchronous
vrep.simxSynchronous(self.clientID, True)
#Start simulation if it didn't start
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
setVelocity(self.clientID, self.LUM, self.LUMSpeed)
setVelocity(self.clientID, self.LLM, self.LLMSpeed)
setVelocity(self.clientID, self.RUM, self.RUMSpeed)
setVelocity(self.clientID, self.RLM, self.RLMSpeed)
#Esto es necesario porque la primera observación siempre es incorrecta
self.observation_space()
vrep.simxSynchronousTrigger(self.clientID)
def main(fileName):
#Maing sure no connection is active
vrep.simxFinish(-1)
#Default Port Numeber
portNumb = 19997
#Establish connection
clientID = vrep.simxStart('127.0.0.1', portNumb, True, True, 5000, 5)
#Verify connection
if clientID == -1:
print("Connection to the simluator api could not be established")
print("Make sure the simulator is up and running on port 19997")
else:
parsedInstructions = readInstructions(fileName)
#Set simulation to be Synchonous instead of Asynchronous
vrep.simxSynchronous(clientID, True)
#Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
robotcontroller = robotController(clientID)
print(parsedInstructions)
for secuence in parsedInstructions:
for instruction in secuence:
robotcontroller.moveRobot(instruction[0])
for i in range(instruction[1]):
vrep.simxSynchronousTrigger(clientID)
#Stop_Start_Simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
#This sleep is necesary for the simulation to finish stopping before starting again
time.sleep(2)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
#Should always end by finishing connetions
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(clientID)
def startSimulation(self):
if self.clientId != -1:
#+++++++++++++++++++++++++++++++++++++++++++++
step = 0.005
vrep.simxSetFloatingParameter(
self.clientId, vrep.sim_floatparam_simulation_time_step, step,
vrep.simx_opmode_oneshot)
vrep.simxSynchronous(self.clientId, True)
vrep.simxStartSimulation(self.clientId, vrep.simx_opmode_oneshot)
#init the controller
planeController = PlaneCotroller(self.clientId)
planeController.take_off()
self.pdController = controller.PID(cid=self.clientId,
ori_mode=True)
#create a thread to controll the move of quadcopter
_thread.start_new_thread(self.pdThread, ())
print("thread created")
#to be stable
planeController.loose_jacohand()
# planeController.move_to(planeController.get_object_pos(planeController.copter),True)
# planeController.plane_pos = planeController.get_object_pos(planeController.copter)
# time.sleep(10)
self.run_simulation(planeController)
else:
print('Failed connecting to remote API server')
print('Simulation ended')
vrep.simxStopSimulation(self.clientId, vrep.simx_opmode_oneshot)
def run_simulation(self):
self.set_num_steps()
vrep.simxSynchronous(self._clientID,True);
vrep.simxStartSimulation(self._clientID,vrep.simx_opmode_oneshot);
for simStep in range (self._num_steps):
self._pid = set_target_thetas(self._num_steps, self._pid,self._experiment,self._simulator,simStep)
for joint in range(6):
errorCode, self._theta[joint] = vrep.simxGetJointPosition(self._clientID,self._arm_joints[joint],vrep.simx_opmode_blocking)
self._linearVelocity[joint] = self._pid[joint].get_velocity(math.degrees(self._theta[joint]))/self._convertdeg2rad
vrep.simxSetJointTargetVelocity(self._clientID,self._arm_joints[joint],self._linearVelocity[joint],vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self._clientID)
vrep.simxGetPingTime(self._clientID)
self._positions.append(vrep.simxGetObjectPosition(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectOrientation(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectPosition(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectQuaternion(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1])
vrep.simxStopSimulation(self._clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(self._clientID)
saveStats(self._experiment,self._current_iteration, self._physics_engine,self._simulator, self._positions)
def __init__(self, max_actions=600):
self.max_actions = max_actions
self.portNumb = recoverPorts()[0]
self.actions = []
self.score = 0
self.hasfallen = False
# self.LUMSpeed = 0
# self.LLMSpeed = 0
# self.RUMSpeed = 0
# self.RLMSpeed = 0
self.clientID = vrep.simxStart('127.0.0.1', self.portNumb, True, True, 5000, 5)
if self.clientID != -1 :
print ("Connection to the simulator has been established")
self.robotController = robotController(self.clientID)
# LUM,LLM,RUM,RLM,head = recoverRobotParts(self.clientID)
# self.LUM = LUM
# self.LLM = LLM
# self.RUM = RUM
# self.RLM = RLM
# self.head = head
#Set simulation to be Synchonous instead of Asynchronous
vrep.simxSynchronous(self.clientID, True)
#Start simulation if it didn't start
vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_blocking)
self.robotController.resetRobot()
# setVelocity(self.clientID,self.LUM,self.LUMSpeed)
# setVelocity(self.clientID,self.LLM,self.LLMSpeed)
# setVelocity(self.clientID,self.RUM,self.RUMSpeed)
# setVelocity(self.clientID,self.RLM,self.RLMSpeed)
#Esto es necesario porque la primera observación siempre es incorrecta
self.observation_space()
vrep.simxSynchronousTrigger(self.clientID)
def initialize():
global clientID
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
# Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
res, v1 = vrep.simxGetObjectHandle(clientID, 'Quadricopter',
vrep.simx_opmode_oneshot_wait)
print(res)
rc, sv = vrep.simxGetFloatSignal(clientID, 'particlesTargetVelocities',
vrep.simx_opmode_oneshot_wait)
print(rc, sv)
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, True)
# load the quadroter model
vrep.simxLoadModel(clientID, "C:\quadrotor.ttm", 0,
vrep.simx_opmode_blocking)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
#functional/handle code:
# with Listener(
# on_press=on_press,
# on_release=on_release) as listener:
# listener.join()
return clientID
else:
print('Not Connected')
sys.exit('Not Connected')
return -1
def start_sim(self): # vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronous(self.clientID, True) dt = .001 vrep.simxSetFloatingParameter(self.clientID,vrep.sim_floatparam_simulation_time_step,dt,vrep.simx_opmode_oneshot) vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot); return
def reinitialization_process(conf):
vrep.simxStopSimulation(conf['clientID'], vrep.simx_opmode_oneshot_wait)
time.sleep(1.)
vrep.simxSynchronous(conf['clientID'],True)
return_code = vrep.simxStartSimulation(conf['clientID'], vrep.simx_opmode_oneshot)# However, v-rep will do strange initialization.
while return_code != 0:
return_code = vrep.simxStartSimulation(conf['clientID'], vrep.simx_opmode_oneshot)
# vrep.simxSetStringSignal(clientID,'jacoHand','true',vrep.simx_opmode_oneshot)
# starting streaming mode
for i in range(conf['num_particles']):
starting_streaming_buffer_mode(conf['clientID'], conf['particle_handles'][i], 'simxGetObjectPosition')
starting_streaming_buffer_mode(conf['clientID'], conf['cup_handle'], 'simxGetObjectPosition')
starting_streaming_buffer_mode(conf['clientID'], conf['cup_handle'], 'simxGetObjectOrientation')
starting_streaming_buffer_mode(conf['clientID'], conf['end_effector_handle'], 'simxGetObjectOrientation')
starting_streaming_buffer_mode(conf['clientID'], conf['end_effector_handle'], 'simxGetObjectPosition')
# print('STREAMING_MODE FINISHED')
for i in range(len(conf['joint_handles'])):
returnCode = vrep.simxSetJointTargetPosition(conf['clientID'], conf['joint_handles'][i], conf['initial_joint_angles'][i], vrep.simx_opmode_streaming)
if returnCode != vrep.simx_return_ok:
print('Initialization of the joint angles fails.' + str(returnCode))
for i in range(round(0.03/conf['dt'])):
return_code = vrep.simxSynchronousTrigger(conf['clientID'])
# get cup position
returnCode, cup_position = vrep.simxGetObjectPosition(conf['clientID'], conf['cup_handle'], -1, vrep.simx_opmode_buffer)
if returnCode != 0:
print('Can not find Cup positions!')
# Distribute different positions to particles according to the position of liquid
for i in range(len(conf['particle_handles'])):
returnCode = vrep.simxSetObjectPosition(conf['clientID'], conf['particle_handles'][i], -1, np.array([cup_position[0],cup_position[1],cup_position[2] + 0.022* i]), vrep.simx_opmode_oneshot)
if returnCode != 0:
print('Can not set particle position! Return code: %i' %returnCode)
for i in range(round(0.7/conf['dt'])):
return_code = vrep.simxSynchronousTrigger(conf['clientID'])
def sim_connect(self):
vrep.simxFinish(-1)
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if self.clientID != -1:
vrep.simxSynchronous(self.clientID, True)
else:
print("Failed to connect to remote API server.")
def startSim():
global clientID, arm_joint, mobile_joint, joint_handles, gripper_handles, throttle_handles, body_handle, sphere_handle
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
deltaTime, # specify a simulation time step
vrep.simx_opmode_oneshot)
# --------------------- Start the simulation
# start our simulation in lockstep with our code
vrep.simxSynchronous(clientID, True)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
joint_handles = [
vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking)[1]
for name in arm_joint
]
gripper_handles = [
vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking)[1]
for name in gripper_joint
]
throttle_handles = [
vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking)[1]
for name in mobile_joint
]
body_handle = vrep.simxGetObjectHandle(clientID, body_name,
vrep.simx_opmode_blocking)[1]
sphere_handle = vrep.simxGetObjectHandle(clientID, spherical_target,
vrep.simx_opmode_blocking)[1]
def __init__(self):
vrep.simxFinish(-1)
self.clientId = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
if self.clientId != -1:
print('Connected to remote API server')
else:
print('Connection to V-Rep server failed')
sys.exit('Could Not Connect')
vrep.simxSynchronous(self.clientId,True)
self.bounceCount = 0
vrep.simxStopSimulation(self.clientId, vrep.simx_opmode_oneshot)
self.get_handles()
self.reset()
k = 0.02
self.move_dict = {0:[k,0,0],1:[-k,0,0],2:[0,k,0],3:[0,-k,0],4:[0,0,2*k],5:[0,0,-2*k]}
self.observation_dimensions = 7
self.action_space = 6
self.incremental_step_count = 0
self.incremental_step = []
self.prev_ball_pos = []
self.bounceFlag = False
self.prevBounceFlag = False
def _connect_to_vrep(self):
# Initialise Communication Line #
portNb = 19997
vrep.simxFinish(-1)
self.clientID = vrep.simxStart('127.0.0.1', portNb, True, True, 2000,
5)
# Connect to vrep
if self.clientID != -1:
print('connection made')
_, self.jointHandles[0] = vrep.simxGetObjectHandle(
self.clientID, 'Shoulder', vrep.simx_opmode_blocking)
_, self.tipHandle = vrep.simxGetObjectHandle(
self.clientID, 'tip', vrep.simx_opmode_blocking)
vrep.simxSynchronous(
self.clientID,
True) # Enable the synchronous mode (Blocking function call)
else:
print('no connection')
# set time-step
vrep.simxSetFloatingParameter(self.clientID,
vrep.sim_floatparam_simulation_time_step,
self._hyperparams['dt'],
vrep.simx_opmode_blocking)
def __init__(self):
self.tempo = 10
print('Programa VREP iniciou a simulação')
#vrep.simxFinish(-1)
self.cliente = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
vrep.simxSynchronous(self.cliente, True)
vrep.simxStartSimulation(self.cliente, vrep.simx_opmode_blocking)
def start(self, synchronous=True):
vrep.simxSynchronous(self._client_id, synchronous)
vrep.simxStartSimulation(self._client_id, vrep.simx_opmode_oneshot)
self.get_agents()
self.get_doors()
self.get_exits()
self.get_monitors()
self._static_goals = [self._doors, self._exits, self._monitors]
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 runModel(portNumber, secuenceList):
#Establish connection
clientID = vrep.simxStart('127.0.0.1', portNumber, True, True, 5000, 5)
#Verify connection
if clientID == -1 :
print("Connection to the simluator api could not be established")
print("Make sure the simulator is up and running on port {}".format(portNumber))
else:
evolutionModel = EvolutionModel()
robotcontroller = robotController(clientID)
#Set simulation to be Synchronous instead of Asynchronous
vrep.simxSynchronous(clientID, True)
#Setting Time Step to 50 ms (miliseconds)
dt = 0.05
#Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
runInfo = []
for secuence in secuenceList:
runtime = 0
observationTrace = []
for instruction in secuence:
robotcontroller.moveRobot(instruction[0])
#This is what makes the simulation Synchronous
initialTime = 0.0
actualTime = initialTime + dt
runtime += dt
#Condition to stop simulation
done = False
vrep.simxSynchronousTrigger(clientID)
n = 1
while (n <= instruction[1] and not done):
n+=1
#Make de simulation run one step (dt determines how many seconds pass by between step and step)
vrep.simxSynchronousTrigger(clientID)
#Advance time in my internal counter
actualTime = actualTime + dt
runtime += dt
observations = robotcontroller.observablePositions()
observationTrace.append({'Observation': observations, 'runtime' : runtime})
done = evolutionModel.isDone(observations)
runInfo.append({'instructions' : secuence, 'observations' : observationTrace, 'Score' : evolutionModel.getScore(observationTrace)})
robotcontroller.resetRobot()
#Stop_Start_Simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
#This sleep is necesary for the simulation to finish stopping before starting again
time.sleep(2)
robotcontroller.resetRobot()
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
robotcontroller.resetRobot()
#Should always end by finishing connetions
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(clientID)
return runInfo
def reset(self):
super(StandingUpEnvironment, self).reset()
# print('*** Reset ***')
vrep.simxStopSimulation(self.client_id, self.opmode)
vrep.simxCloseScene(self.client_id, self.opmode)
vrep.simxLoadScene(self.client_id, self.model_path, 0, self.opmode)
vrep.simxSynchronous(self.client_id, True)
vrep.simxStartSimulation(self.client_id, self.opmode)
self.bioloid = Bioloid(self.client_id)
def streamVisionSensor(visionSensorName, clientID, pause=0.0001):
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, 1)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# enable streaming of the iteration counter:
res, iteration1 = vrep.simxGetIntegerSignal(clientID, "iteration",
vrep.simx_opmode_streaming)
print("iteration1: ", iteration1)
#Get the handle of the vision sensor
res1, visionSensorHandle = vrep.simxGetObjectHandle(
clientID, visionSensorName, vrep.simx_opmode_oneshot_wait)
#Get the image
res2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_streaming)
#Initialiazation of the figure
time.sleep(0.5)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
# Init figure
plt.ion()
fig = plt.figure(1)
plotimg = plt.imshow(im, origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
while (vrep.simxGetConnectionId(clientID) != -1):
if iteration1 < 30:
vrep.simxSynchronousTrigger(clientID)
res, iteration1 = vrep.simxGetIntegerSignal(
clientID, "iteration", vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok: iteration1 = -1
iteration2 = iteration1
while iteration2 == iteration1: # wait until the iteration counter has changed
res, iteration2 = vrep.simxGetIntegerSignal(
clientID, "iteration", vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok:
iteration2 = -1
print(iteration2)
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed
img = np.array(image, dtype=np.uint8).reshape(
[resolution[1], resolution[0], 3])
#Update the image
fig.canvas.set_window_title(visionSensorName + '_' +
str(iteration2))
plotimg.set_data(img)
plt.draw()
plt.pause(pause)
# vrep.simxSynchronousTrigger(clientID)
print('End of Simulation')
def start_sim(self):
# Set Simulator
vrep.simxSynchronous(self.clientID, True)
dt = .05
vrep.simxSetFloatingParameter(self.clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
return
def reset( self ):
err = vrep.simxStopSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
time.sleep(1)
self.pos_err = [0,0,0]
self.ori_err = [0,0,0]
self.lin = [0,0,0]
self.ang = [0,0,0]
err = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
if SYNC:
vrep.simxSynchronous( self.cid, True )
def reset(self):
err = vrep.simxStopSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
time.sleep(1)
self.pos_err = [0, 0, 0]
self.ori_err = [0, 0, 0]
self.lin = [0, 0, 0]
self.ang = [0, 0, 0]
err = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
if SYNC:
vrep.simxSynchronous(self.cid, True)
def finishSimulation(self, clientID):
self.getObjectPositionFirstTime = True
errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait)
error=errorStop or errorClose
errorFinish=vrep.simxFinish(clientID)
error=error or errorFinish
if self.synchronous:
vrep.simxSynchronous(clientID,False)
return error
def __init__(self, connectionPort=19999):
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP
self.joint_handlers = OrderedDict()
if self.clientID == -1:
print('Failed connecting to remote API server')
exit(1)
print('Connected to remote API server')
vrep.simxSynchronous(self.clientID,True)
vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_blocking)
def wait_until_simulator_started(clientID):
vrep.simxSynchronous(clientID, True)
while True:
try:
if vrep.simxStartSimulation(
clientID,
vrep.simx_opmode_oneshot_wait) == vrep.simx_return_ok:
# print('restart succeessfully')
break
except KeyboardInterrupt:
sys.exit('Program Ended')
def start_session(vrep_host='127.0.0.1', vrep_port=19999):
'''start a communication session'''
global clientID
vrep.simxFinish(-1) # clean up communication threads
clientID = vrep.simxStart(vrep_host, vrep_port, True, True, 5000, 5)
if clientID == -1:
print('Failed to connect to V-rep!')
exit()
print ('Connected to V-rep at %s:%d' % (vrep_host, vrep_port))
vrep.simxSynchronous(clientID, True)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
def start_simulation(self):
"""
Starts the simulation and sets the according parameters
"""
vrep.simxSynchronous(self.client_id, True)
if self.headless:
self.set_boolean_parameter(vrep.sim_boolparam_threaded_rendering_enabled, True)
vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_blocking)
self.running = True
time.sleep(self.start_stop_delay)
def connect(self):
global SYNC
self.cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP
if self.cid != -1:
print ('Connected to V-REP remote API serv'
'\er, client id: %s' % self.cid)
vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot)
if SYNC:
vrep.simxSynchronous(self.cid, True)
else:
print ('Failed connecting to V-REP remote API server')
exit()
def connect():
cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
if cid != -1:
print('Connected to V-REP remote API serv' '\er, client id: %s' % cid)
vrep.simxStartSimulation(cid, vrep.simx_opmode_oneshot)
if SYNC:
vrep.simxSynchronous(cid, True)
else:
print('Failed connecting to V-REP remote API server')
exit()
return cid
def _init_vrep(self): # get clientID
vrep.simxFinish(-1) # end all running communication threads
clientID = vrep.simxStart('127.0.0.1', 19997, True, False, 5000, 0) # get the clientID to communicate with the current V-REP
self.clientID = clientID
if self.clientID != -1: # clientID = -1 means connection failed.(time-out)
print('\n' + 'Connected to remote V-REP server.')
else:
print('\n' + 'Connection time-out !')
raise Exception('Connection Failed !')
vrep.simxSynchronous(self.clientID, True) # enable synchronous operation to V-REP
def make_connection():
vrep.simxFinish(-1) #close all open connections
clientID = vrep.simxStart('127.0.0.1',19997,True,True,5000,5) #v-rep connections
vrep.simxSynchronous(clientID,True)
# start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)
if clientID == -1:
print('connection failed')
exit()
else:
print('connection succeeded')
return clientID
def connectToServer(self):
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)
if self.clientID != -1: # if we connected successfully
print('Connected to remote API server')
time.sleep(1)
#Setup synchronous mode or not
if self.synchronous == True:
print("In synchronous mode")
vrep.simxSynchronous(self.clientID, True)
dt = 0.001
time.sleep(1)
def reset( self ):
err = vrep.simxStopSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
time.sleep(1)
self.pos_err = [0,0,0]
self.ori_err = [0,0,0]
self.lin = [0,0,0]
self.ang = [0,0,0]
self.vert_prox = 0
self.left_prox = 0
self.right_prox = 0
err = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
if self.sync:
vrep.simxSynchronous( self.cid, True )
def connectVREP():
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected Remote Api')
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronous(clientID, True)
return clientID
else:
print('ERROR! Error connecting Remote Api')
sys.exit(0)
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)
if int(self.sysConf.getProperty("blank screen?"))==1:
vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)
return error
def startsim(self):
"""
Start the simulation in synchronous mode to achieve exact simulation
independent of the framerate. To be able to do this, you have to
connec to the port 19997 of vrep, edit the file
`remoteApiConnections.txt` to contain something like
portIndex1_port = 19997
portIndex1_debug = true
portIndex1_syncSimTrigger = false
"""
self.set_led(8,0)
vrep.simxStartSimulation(self._clientID, vrep.simx_opmode_oneshot)
vrep.simxSynchronous(self._clientID, True)
self.stepsim(1)
def __init__(self, sim_dt=0.05, nengo_dt=0.001, sync=True):
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
self.sync = sync
if self.cid != -1:
print ('Connected to V-REP remote API server, client id: %s' % self.cid)
vrep.simxStartSimulation( self.cid, vrep.simx_opmode_oneshot )
if self.sync:
vrep.simxSynchronous( self.cid, True )
else:
print ('Failed connecting to V-REP remote API server')
exit(1)
self.count = 0
self.sim_dt = sim_dt
self.nengo_dt = nengo_dt
def __init__(self):
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(0.5)
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # tenta conectar no simulador, se salva o clientID
# paramos a simulacao, se estiver em andamento, e comecamos denovo
# vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
#time.sleep(0.5)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# modo da API, como eh False, esta no modo assincrono(os ticks da simulacao rodam em velocidade independente)
vrep.simxSynchronous(self.clientID, False)
print("connected with id ", self.clientID)
self.oldtarget = None
self.camera = None
self.setup()
self.lastimageAcquisitionTime = 0
def connection(self):
"""
Make connection with v-rep simulator
"""
print ('Waiting for connection...')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',self.port,True,True,5000,5) # Connect to V-REP
if self.clientID!=-1:
print ('Connected to remote API server')
# enable the synchronous mode on the client:
if self.synchronous:
vrep.simxSynchronous(self.clientID,True)
# start the simulation:
vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
else:
raise IOError('Failed connecting to remote API server')
def start(self):
returnCode = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
if (returnCode>1):
print "returnCode: ", returnCode
raise Exception('Could not start')
returnCode=vrep.simxSynchronous(self.clientID,True)
for k in range(10): #Run to steps to step through initial "drop"
returncode = vrep.simxSynchronousTrigger(self.clientID)
if (returnCode!=0):
raise Exception('Could not set synchronous mode')
def VREPConnect(self):
vrep.simxFinish(-1)
"Connect to Simulation"
self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5)
eCode = vrep.simxSynchronous(self.simulID, True)
if eCode != 0:
print "Could not get V-REP to synchronize operation with me"
if not self.simulID == -1:
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
print "my SimulID is ", self.simulID
else:
sys.exit("Failed to connect to VREP simulation. Bailing out")
def connectToVREP(self, VREP_World=None):
"Connect to VREP and load the correct world if needed"
"FIXME: SHOULD LAUNCH VREP IF NOT RUNNING"
VREP_exec = 'vrep'
if self.VREPSimulation == None:
self.VREPSimulation = VREP_World
'1. check that V-Rep is running and see whether we are already connected to it. Otherwise connect'
if VREP_exec not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]):
raise Exception(("V-REP is not running! Please start V-REP with scene: %s" % self.VREPSimulation))
else:
"Check if we are connected with the passed clientId already"
if self._VREP_clientId is not None:
print "ClientId = " ,self._VREP_clientId
connId = vrep.simxGetConnectionId(self._VREP_clientId)
print "My connId is " , connId
if connId == -1: # we are not: set client Id to none and re-connect
print "Disconnecting all existing connections to V-REP"
vrep.simxFinish(-1)
self._VREP_clientId = None
while self._VREP_clientId is None:
self._VREP_clientId = vrep.simxStart(self._host, self._kheperaPort,True,True, 5000,5)
if not self._VREP_clientId == -1:
eCode = vrep.simxSynchronous(self._VREP_clientId, True)
if eCode != 0:
raise Exception("Failed to connect to VREP simulation. Bailing out")
# print " we are connected with clientId ", self._VREP_clientId
"2. Check the correct world is running"
if self.VREPSimulation is not None:
VREP_Scene = split(self.VREPSimulation)[1]
if VREP_Scene not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]):
eCode = vrep.simxLoadScene(self._VREP_clientId, self.VREPSimulation, 0, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception(("Could not load into V-REP the world", self.VREPSimulation))
"3. Make sure VREP has bees set to the correct directory for saving data"
self.setDataDir(self.dataDir)
'4. Start simulation'
eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("VREP simulation cannot get started")
else:
print "V-REP simulation is running with clientId: ", self._VREP_clientId
return self._VREP_clientId
def connect(self):
#os.chdir(VREP_HOME)
#subprocess.call([os.path.join(VREP_HOME,'vrep.sh'), VREP_scene_file], shell = True, cwd = VREP_HOME)
"Close existing connections"
vrep.simxFinish(-1)
"Connect to Simulation"
self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5)
eCode = vrep.simxSynchronous(self.simulID, True)
if eCode != 0:
print "Could not get V-REP to synchronize operation with me"
if not self.simulID == -1:
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
print "my SimulID is ", self.simulID
else:
sys.exit("Failed to connect to VREP simulation. Bailing out")
def start_simulation(self):
rc = vrep.simxStartSimulation(self.client_id,
vrep.simx_opmode_oneshot_wait)
rc = vrep.simxSynchronous(self.client_id, True)
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "vrep.py"')
print ('--------------------------------------------------------------')
print ('')
import time
import sys
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to V-REP
if clientID!=-1:
print ('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID,True)
# start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# Now step a few times:
for i in range(1,10):
if sys.version_info[0] == 3:
input('Press <enter> key to step the simulation!')
else:
raw_input('Press <enter> key to step the simulation!')
vrep.simxSynchronousTrigger(clientID);
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
import vrep
from time import sleep
vrep.simxFinish(-1)
simulID = vrep.simxStart('127.0.0.1',19997,True,True, 5000,5)
vrep.simxSynchronous(simulID, True)
for i in xrange(5):
print "starting no : ", i
print "now starting"
vrep.simxStartSimulation(simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(simulID)
sleep(4)
print "now stopping"
vrep.simxStopSimulation(simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(simulID)
sleep(2)
print "done with run no: ", i
def __init__( self, max_target_distance=4, noise=False,
noise_std=None, dodging=True,
target_func=None, cid=None
):
# If a cid is specified, assume the connection has already been
# established and should remain open
if cid is None:
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
else:
self.cid = cid
if self.cid != -1:
print ('Connected to V-REP remote API server, client id: %s' % self.cid)
vrep.simxStartSimulation( self.cid, vrep.simx_opmode_oneshot )
if SYNC:
vrep.simxSynchronous( self.cid, True )
else:
print ('Failed connecting to V-REP remote API server')
self.exit()
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait )
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait )
# Reset the motor commands to zero
packedData=vrep.simxPackFloats([0,0,0,0])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
self.pos = [0,0,0]
self.pos_err = [0,0,0]
self.t_pos = [0,0,0]
self.lin = [0,0,0]
self.ori = [0,0,0]
self.ori_err = [0,0,0]
self.t_ori = [0,0,0]
self.ang = [0,0,0]
self.count = 0
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
if noise_std is not None:
self.noise = True
else:
self.noise = False
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
# Overwrite the get_target method if the target is to be controlled by a
# function instead of by V-REP
if target_func is not None:
self.step = 0
self.target_func = target_func
def get_target():
self.t_pos, self.t_ori = self.target_func( self.step )
self.step += 1
self.get_target = get_target
def start_simulation(self):
mode = vrep.simx_opmode_oneshot_wait
assert vrep.simxStartSimulation(self.clientID, mode) == 0,"StartSim Error"
assert vrep.simxSynchronous(self.clientID, True) == 0,"Sync Error"
self.config_handles()
# Handles of body and wheels
e, body = vrep.simxGetObjectHandle(client_id, "body", vrep.simx_opmode_oneshot_wait)
e, joint_0 = vrep.simxGetObjectHandle(client_id, "joint_0", vrep.simx_opmode_oneshot_wait)
e, joint_1 = vrep.simxGetObjectHandle(client_id, "joint_1", vrep.simx_opmode_oneshot_wait)
# Set the joint angles to the default position (0, 0)
e = vrep.simxSetJointTargetPosition(client_id, joint_0, 0, vrep.simx_opmode_streaming)
e = vrep.simxSetJointTargetPosition(client_id, joint_1, 0, vrep.simx_opmode_streaming)
# this line is necessary to enable position recording
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_streaming)
# Wait until the robot is settled to the default position
time.sleep(0.3)
# enable synchronous mode
vrep.simxSynchronous(client_id, True)
position_history = []
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer)
position_history.append(body_pos)
joint_pos_history = []
e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_streaming)
e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_streaming)
joint_pos_history.append([joint_0_pos, joint_1_pos])
with contexttimer.Timer() as timer:
for i in range(4):
e = vrep.simxSetJointTargetPosition(client_id, joint_0, 0.5, vrep.simx_opmode_streaming)
for t in range(3):
vrep.simxSynchronousTrigger(client_id)
def connect(self, ip='127.0.0.1', port=19997):
self.clientID = vrep.simxStart(ip, port, True, True, 5000, 5)
vrep.simxSynchronous(self.clientID, True)
self.startSimulation()
