def startvrep():
global clientID2
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(flask_ip, vrep_port, True, True, 5000,
5) # Get the client ID
clientID2 = sim.simxStart(flask_ip, vrep_port2, True, True, 5000, 5)
res = sim.simxLoadScene(clientID, scene_path, 0, sim.simx_opmode_blocking)
x = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
if clientID != -1 or clientID2 != -1: #check if client connection successful
print('Connected to remote API server')
else:
print('Connection not successful')
sys.exit('Could not connect')
# Initialize car control object
AirHockey = air_Hockey(clientID)
#for i in range(150):
while True:
# Start time for image process
err, img = AirHockey.get_image()
ret, jpeg = cv2.imencode('.jpg', img)
#jpeg.tobytes()
yield (b'--frame\r\n'
b'Content-Type: image/jpeg\r\n\r\n' + jpeg.tobytes() + b'\r\n')
return "startvrep"
def create_dataset(path):
try:
shutil.rmtree(path + "/mav0")
except:
print("Removed before")
os.makedirs(path + "/mav0/cam0/data")
os.makedirs(path + "/mav0/cam1/data")
print('Argument List:', str(sys.argv[1]))
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
print('Connected to remote API server')
#Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
er, t_rightWheel = sim.simxGetObjectHandle(clientID,
'Pioneer_p3dx_rightMotor',
sim.simx_opmode_blocking)
er, t_leftWheel = sim.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor',
sim.simx_opmode_blocking)
er, cam_handle_left = sim.simxGetObjectHandle(
clientID, 'anaglyphStereoSensor_leftSensor',
sim.simx_opmode_blocking)
er, cam_handle_right = sim.simxGetObjectHandle(
clientID, 'anaglyphStereoSensor_rightSensor',
sim.simx_opmode_blocking)
i = 0
fl = open(path + "/mav0/timestamps.txt", "w")
while (True):
err, resolution_left, colorCam_left = sim.simxGetVisionSensorImage(
clientID, cam_handle_left, 0, sim.simx_opmode_oneshot_wait)
err, resolution_right, colorCam_right = sim.simxGetVisionSensorImage(
clientID, cam_handle_right, 0, sim.simx_opmode_oneshot_wait)
img_left_1 = np.array(colorCam_left, dtype=np.uint8)
img_right_1 = np.array(colorCam_right, dtype=np.uint8)
img_left_1.resize([resolution_left[1], resolution_left[0], 3])
img_right_1.resize([resolution_right[1], resolution_right[0], 3])
img_left_2 = np.flipud(img_left_1)
img_right_2 = np.flipud(img_right_1)
img_left_3 = img_left_2[..., ::-1].copy()
img_right_3 = img_right_2[..., ::-1].copy()
milli_time = int(round(time.time() * 1000)) * 1000000000
st = str(milli_time) + '\n'
st_left = path + '/mav0/cam0/data/{}.png'.format(milli_time)
st_right = path + '/mav0/cam1/data/{}.png'.format(milli_time)
cv2.imwrite(st_left, img_left_3)
cv2.imwrite(st_right, img_right_3)
cv2.waitKey(1)
i = i + 1
fl.write(st)
sim.simxSetJointTargetVelocity(clientID, t_rightWheel, 1,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID, t_leftWheel, 1,
sim.simx_opmode_streaming)
fl.close()
def vrepInterface(port):
global angles_handler
angles_handler = np.zeros(12)
global angles_error
angles_error = np.zeros(12)
global clientID
clientID = 0
global inital_name
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
ID = sim.simxStart('127.0.0.1', port, True, True, 5000,
5) # Connect to V-REP
print(ID)
if ID != -1:
print('Connected to remote API server')
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(ID, sim.sim_handle_all,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
else:
print('Remote API function call returned with error code: ', res)
else:
print('DIDNOT CONNECT!!!')
intial_name = [
'ab3', 'bc3', 'cd3', 'ab4', 'bc4', 'cd4', 'ab1', 'bc1', 'cd1', 'ab2',
'bc2', 'cd2'
]
for i in range(angles_handler.shape[0]):
angles_error[i], angles_handler[i] = sim.simxGetObjectHandle(
clientID, intial_name[i], sim.simx_opmode_blocking)
return ID
def __init__(self):
ip = '127.0.0.1'
port = 19997
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart(ip, port, True, True, -5000, 5)
if self.clientID == -1:
import sys
sys.exit('\nV-REP remote API server connection failed (' + ip +
':' + str(port) + '). Is V-REP running?')
print('Connected to Remote API Server')
with open(
'primitive_base/dance_primitive_library_interpole.json') as f:
self.dance_primitive_library = json.load(f)
self.Body = {}
get_first_handles(
self.clientID,
self.Body) #get first handles of Nao in the virtual environment
self.joint_actuator = Joint_Actuator(self.clientID, self.Body)
self.init_time = time.time()
self.time_sleep = 0.01
self.i = 0
def init_remote_api_server():
"""
Purpose:
---
This function should first close any open connections and then start
communication thread with server i.e. CoppeliaSim.
NOTE: In this Task, do not call the exit_remote_api_server function in case of failed connection to the server.
The test_task_2a executable script will handle that condition.
Input Arguments:
---
None
Returns:
---
`client_id` : [ integer ]
the client_id generated from start connection remote API, it should be stored in a global variable
Example call:
---
client_id = init_remote_api_server()
NOTE: This function will be automatically called by test_task_2a executable before starting the simulation.
"""
global client_id
############## ADD YOUR CODE HERE ##############
sim.simxFinish(-1)
client_id = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
##################################################
return client_id
def __init__(self):
try:
sim.simxFinish(-1) #close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
print('connect successfully')
else:
sys.exit("connect error")
except:
print('Check if CoppeliaSim is open')
_, Quadcopter_target = sim.simxGetObjectHandle(
clientID, 'Quadricopter_target', sim.simx_opmode_blocking)
_, targetPosition = sim.simxGetObjectPosition(clientID,
Quadcopter_target, -1,
sim.simx_opmode_buffer)
print(targetPosition)
ArrayPosition = [-0.18570, 0.99366, 0.615]
sim.simxSetObjectPosition(clientID, Quadcopter_target, -1,
ArrayPosition, sim.simx_opmode_oneshot)
self.clientID = clientID
self.Quadcopter_target = Quadcopter_target
self.targetPosition = targetPosition
def conect_and_load(port, ip):
clientID = sim.simxStart(ip, port, True, True, 5000,
5) # Connect to CoppeliaSim
scene_path = os.path.join('vrep-scene', 'poppy_two_target_pos_z.ttt')
print('-' * 5, 'Scene path:', scene_path, '-' * 5)
sim.simxLoadScene(clientID, scene_path, 0xFF, sim.simx_opmode_blocking)
return clientID
def onConnect(self):
if self.clientID != -1:
return
self.clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if self.clientID == -1:
messagebox.showinfo("No se puede conectar")
else:
for i in range(5):
j = i + 1
nom = self.base + str(j)
_, handle = sim.simxGetObjectHandle(self.clientID, nom,
sim.simx_opmode_blocking)
self.handles.append(handle)
_, self.j1 = sim.simxGetObjectHandle(self.clientID,
'youBotGripperJoint1',
sim.simx_opmode_blocking)
_, self.j2 = sim.simxGetObjectHandle(self.clientID,
'youBotGripperJoint2',
sim.simx_opmode_blocking)
print("Conectado y envio comando al motor")
print(self.j1, self.j2)
self.init_robot()
def __init__(self, robot, host="127.0.0.1", port=19997):
self.robot = robot
self.sim_joints = [0, 0, 0, 0, 0, 0]
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart(host, port, True, True, 5000,
5) # Connect to CoppeliaSim
if self.clientID != -1:
print('Connected to remote API server')
else:
print("coppeliasim connection failed")
return
res, objs = sim.simxGetObjects(self.clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
else:
print('Remote API function call returned with error code: ', res)
self.jh = [0, 0, 0, 0, 0, 0]
for i in range(6):
res, self.jh[i] = sim.simxGetObjectHandle(
self.clientID, 'UR5_joint{}'.format(i + 1),
sim.simx_opmode_blocking)
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
def __enter__(self):
self.executedMovId1 = 'notReady'
self.executedMovId2 = 'notReady'
sim.simxFinish(-1) # just in case, close all opened connections
self.id = sim.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to CoppeliaSim
return self
def start_Simulation(self):
print('Program started')
# Just in case, close all opened connections
sim.simxFinish(-1)
# Connect to CoppeliaSim, as continuous remote API Server
self.clientID = sim.simxStart('127.0.0.1', 19997, False, True, 5000, 5)
if self.clientID != -1:
print('Connected to remote API server')
#Start simulation
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
# Now send some data to CoppeliaSim in a non-blocking fashion:
sim.simxAddStatusbarMessage(self.clientID, 'Connection Succeed!',
sim.simx_opmode_oneshot)
# Before closing the connection to CoppeliaSim, make sure that the last command sent
# out had time to arrive. You can guarantee this with (for example):
sim.simxGetPingTime(self.clientID)
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(self.clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
else:
print('Remote API function call returned with error code: ',
res)
time.sleep(2)
else:
print('Failed connecting to remote API server')
return self.clientID
def __init__(self):
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if self.clientID != -1:
print('Connected to remote API server')
else:
print('Failed connecting to remote API server')
def startStopSim():
global PORT, client, cameras
if client is None:
# No client, then Start
PORT = int(request.args.get('port'))
print("waiting")
while portpicker.is_port_free(PORT):
sleep(5 * againIn)
print(".")
client = sim.simxStart('127.0.0.1', PORT, True, True, 5000, 5)
verboseResp(client, "simxStart")
return str(client)
else:
# There is a client, then Stop
stop()
sleep(.5)
response = sim.simxSetIntegerSignal(client, 'doClose', 1,
sim.simx_opmode_oneshot_wait)
verboseResp(response, "simxSetIntegerSignal doClose")
try:
finish()
except:
verboseResp("already finished?", "finish")
reset()
if response == -1:
return "-1"
else:
return "0"
def conect_and_load(port, ip):
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(ip,port,True,True,5000,5) # Connect to CoppeliaSim
scene_path = os.path.join('vrep_scene', 'hexapod_scene.ttt')
print('-'*5, 'Scene path:', scene_path, '-'*5)
sim.simxLoadScene(clientID, scene_path, 0xFF, sim.simx_opmode_blocking)
return clientID
def __enter__(self):
self.intSignalName='legacyRemoteApiStepCounter'
self.stepCounter=0
self.maxForce=100
sim.simxFinish(-1) # just in case, close all opened connections
self.id=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim
return self
def __init__(self):
sim.simxFinish(-1)
self.clientID = sim.simxStart(self.DOCKER_IP, 5555, True, True, 5000,
5)
if self.clientID == -1:
rospy.logerr('Failed connecting to remote API server')
sim.simxFinish(-1)
sys.exit(1)
rospy.loginfo('Connected to remote API server')
rospy.loginfo('Testing connection')
objs = self.run_sim_function(
sim.simxGetObjects,
(self.clientID, sim.sim_handle_all, sim.simx_opmode_blocking))
rospy.loginfo(f'Number of objects in the scene: {len(objs)}')
self.robot = self.run_sim_function(
sim.simxGetObjectHandle,
(self.clientID, "Rob", sim.simx_opmode_blocking))
gate_names = ["Gate", "GateLeftChild", "GateRightChild"]
self.gate = [
self.run_sim_function(
sim.simxGetObjectHandle,
(self.clientID, name, sim.simx_opmode_blocking))
for name in gate_names
]
self.set_position_to_zero()
rospy.sleep(0.1)
self.init_streaming()
rospy.loginfo("Starting main loop")
def __init__(self): # n_actions:3 (target pos), n_states:6 (3pos+3force)
self.metadata = {'render.modes': ['human']}
super().__init__()
sim.simxFinish(-1)
for _ in range(5):
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000,
5)
if self.clientID != -1:
# print('[INFO] Connected to CoppeliaSim.')
break
if self.clientID == -1:
raise IOError('[ERROR] Could not connect to CoppeliaSim.')
sim.simxSynchronous(self.clientID, True)
# sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
self.stepCount = 0
self.reward = 0
self.n_substeps = 10
# self.sim_timestep = 0.5 # set in coppeliaSim (not implemented)
self.n_actions = 3
self.n_states = 6
self.action_space = spaces.Box(-1.,
1,
shape=(self.n_actions, ),
dtype='float32')
self.observation_space = spaces.Box(-np.inf,
np.inf,
shape=(self.n_states, ),
dtype='float32')
self._getHandles()
sim.simxGetPingTime(self.clientID)
def main():
print('### Program started')
print('### Number of arguments:', len(sys.argv), 'arguments.')
print('### Argument List:', str(sys.argv))
sim.simxFinish(-1) # just in case, close all opened connections
port = int(sys.argv[1])
clientID = sim.simxStart('127.0.0.1', port, True, True, 2000, 5)
if clientID == -1:
print('### Failed connecting to remote API server')
else:
print('### Connected to remote API server')
hRobot = getRobotHandles(clientID)
while sim.simxGetConnectionId(clientID) != -1:
# Perception
sonar = getSonar(clientID, hRobot)
#print ('### s', sonar)
blobs, coord = getImageBlob(clientID, hRobot)
nspeed = 1.25
if blobs == 1:
if coord[0] > 0.5:
pd = abs(0.5 - coord[0]) / 0.5
pi = 0
else:
pi = (0.5 - coord[0]) / 0.5
pd = 0
if coord[1] >= 0.6:
res = 0.5
else:
res = 0
print('pd= ', pd, 'pi= ', pi, 'Y= ', coord[1])
lspeed, rspeed = nspeed + (1.5 * pd) - res, nspeed + (1.5 *
pi) - res
else:
lspeed, rspeed = avoid(sonar)
#lspeed, rspeed = +1.5,+0
# Planning
#lspeed, rspeed = avoid(sonar)
# Action
setSpeed(clientID, hRobot, lspeed, rspeed)
time.sleep(0.1)
print('### Finishing...')
sim.simxFinish(clientID)
print('### Program ended')
def __enter__(self):
self.intSignalName = 'legacyRemoteApiStepCounter'
self.stepCounter = 0
self.lastImageAcquisitionTime = -1
sim.simxFinish(-1) # just in case, close all opened connections
self.id = sim.simxStart('192.168.15.55', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
return self
def connect(self):
print ('Programa inicio')
sim.simxFinish(-1) # cerrar todas las conexiones
# Conectar a CoppeliaSim
self.clientID=sim.simxStart(self.ip,self.port,True,True,5000,5)
if(self.clientID == -1):
print("Imposible conectar")
self.activo = False
def __init__(self):
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if self.clientID != -1: #check if client connection successful
print('Connected to remote API server')
# enable the synchronous mode on the client:
sim.simxSynchronous(self.clientID, True)
else:
print('Connection not successful')
sys.exit('Could not connect')
self.Vmax = 0.05
self.Wmax = np.pi / 4
# Action Space
self.action_space = spaces.Discrete(3)
# Observation Space
self.observation_space = spaces.Box(
low=np.array([0, 0, 0., -np.pi / 4, 0, 0, 0, 0, 0, 0, 0, 0],
dtype=np.float32),
high=np.array(
[3, 2 * np.pi, 0.08, np.pi / 4, 1, 1, 1, 1, 1, 1, 1, 1],
dtype=np.float32),
dtype=np.float32)
# Objetcs in the Simulation Scene
errorCode, self.right_motor = sim.simxGetObjectHandle(
self.clientID, 'K4_Right_Motor', sim.simx_opmode_oneshot_wait)
errorCode, self.left_motor = sim.simxGetObjectHandle(
self.clientID, 'K4_Left_Motor', sim.simx_opmode_oneshot_wait)
errorCode, self.khepera = sim.simxGetObjectHandle(
self.clientID, 'Khepera_IV', sim.simx_opmode_oneshot_wait)
errorCode, self.target = sim.simxGetObjectHandle(
self.clientID, 'Target', sim.simx_opmode_oneshot_wait)
errorCode, self.camera = sim.simxGetObjectHandle(
self.clientID, 'Vision_sensor', sim.simx_opmode_oneshot_wait)
self.sensor = {}
for i in range(8):
handle = 'K4_Infrared_{}'.format(i + 1)
errorCode, self.sensor[i] = sim.simxGetObjectHandle(
self.clientID, handle, sim.simx_opmode_oneshot_wait)
self.viewer = None
self.seed()
self.steps = 0
self.radius = 0.8
self.MaxSteps = 800
self.problem = True
self.Velocities = [0, 0]
self.Movements = [[4.285, 0.515], [4.285, 4.285], [0.515, 4.285]]
self.xp, self.yp = self.getPositionTarget()
self.ResetSimulationScene()
self.Randomize = True
self.RobotOrientationRand = True
def __init__(self):
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to CoppeliaSim
if self.clientID != -1:
print('Connected to remote API server')
sim.simxSynchronous(self.clientID, True)
sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
def drone_position(args):
drones = [[] for i in range(3)]
drones_names = [
'Quadricopter_base', 'Quadricopter_base#0', 'Quadricopter_base#1'
]
nodes = []
data = [[] for i in range(3)]
if len(args) > 1:
for n in range(1, len(args)):
nodes.append(args[n])
else:
info("No nodes defined")
exit()
info('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
res = None
info('Connected to remote API server')
# Getting the ID of the drones from the simulation
for i in range(0, len(drones)):
[res,
drones[i]] = sim.simxGetObjectHandle(clientID, drones_names[i],
sim.simx_opmode_oneshot_wait)
if res == sim.simx_return_ok:
info('Connected with CoppeliaSim')
else:
info('Remote API function call returned with error code: ', res)
time.sleep(2)
# Starting the getPosition function streaming
for i in range(0, len(drones)):
sim.simxGetObjectPosition(clientID, drones[i], -1,
sim.simx_opmode_streaming)
while True:
# Getting the positions as buffers
for i in range(0, len(drones)):
# Try to retrieve the streamed data
returnCode, data[i] = sim.simxGetObjectPosition(
clientID, drones[i], -1, sim.simx_opmode_buffer)
# Storing the position in data files
for i in range(0, len(data)):
send_file(data[i], nodes[i])
time.sleep(1)
# Now close the connection to CoppeliaSim:
sim.simxFinish(clientID)
else:
info('Failed connecting to remote API server')
info('Program ended')
def open_connection(self):
sim.simxFinish(-1) # just in case, close all opened connections
self.client_id = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim
if self.client_id != -1:
print('Robot connected')
else:
print('Connection failed')
return self.client_id
def initCon():
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
#comprobamos que se haya podido conectar con CoppeliaSim
if clientID != -1:
print("conexión establecida!")
else:
sys.exit("Error: No se puede conectar")
#ocurre cuando se lanza primero python y después vrep. Debe hacerse al contrario
return clientID
def verificaConexao(self):
conectado = sim.simxGetConnectionId(self.id) != -1
while not conectado:
sim.simxFinish(-1)
self.id = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
conectado = sim.simxGetConnectionId(self.id) != -1
return conectado
def move_right():
clientID2 = sim.simxStart(flask_ip, vrep_port2, True, True, 5000,
5) # Get the client ID
errorCode, player_x_handle = sim.simxGetObjectHandle(
clientID2, 'Pla_X_joint', sim.simx_opmode_oneshot_wait)
sim.simxSetJointTargetVelocity(clientID2, player_x_handle, -1,
sim.simx_opmode_oneshot_wait)
sim.simxFinish(clientID2)
print("move_right")
return ("nothing")
def startSimulation():
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 50)
if clientID != -1:
print('Connected to remote API server')
else:
print('Failed connecting to remote API server')
sys.exit('Could not connect to remote API server')
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
return clientID
def connect(port):
# Establece la conexión a VREP
# port debe coincidir con el puerto de conexión en VREP
# retorna el número de cliente o -1 si no puede establecer conexión
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', port, True, True, 2000,
5) # Conectarse
if clientID == 0: print("conectado a", port)
else: print("no se pudo conectar")
return clientID
def reset():
clientID2 = sim.simxStart(flask_ip, vrep_port2, True, True, 5000,
5) # Get the client ID
sim.simxStopSimulation(clientID2, sim.simx_opmode_oneshot_wait)
time.sleep(1)
sim.simxStartSimulation(clientID2, sim.simx_opmode_oneshot_wait)
time.sleep(0.5)
sim.simxFinish(clientID2)
print("reset")
return ("nothing")
