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 __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 close(self):
sim.simxAddStatusbarMessage(self.clientID, 'Goodbye CoppeliaSim!',
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot)
time.sleep(2)
sim.simxFinish(self.clientID)
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 __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 execute(self):
if(self.activo == False):
return
while(1):
#Guardar frame de la camara, rotarlo y convertirlo a BGR
imgL = self.get_image(self.camLeft)
imgM = self.get_image(self.camMid)
imgR = self.get_image(self.camRight)
#Mostrar frame y salir con "ESC"
cv2.imshow('Left', imgL)
cv2.imshow('Right', imgR)
cv2.imshow('Middle', imgM)
tecla = cv2.waitKey(5) & 0xFF
if tecla == 27:
break
elif tecla != 255:
self.mover_robot_tecladov2(tecla)
#cerrar
sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot)
sim.simxFinish(self.clientID)
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 execute(self):
if (self.activo == False):
return
list_estados = []
model = load_model("modelrobotica")
cont = 0
while (1):
img = self.get_image(self.camSup)
cv2.imshow('Image', img)
image = cv2.resize(img, (32, 32))
image = img_to_array(image)
image = np.array(image, dtype="float") / 255.0
image = image.reshape(-1, 32, 32, 3)
prediction = np.argmax(model.predict(image))
print("model pred: {}".format(prediction), end=" ")
tecla = cv2.waitKey(5) & 0xFF
if tecla == 27:
break
self.vel_follow()
cont = cont + 1
#d = self.get_distance(self.ultra,self.dmax)
#print("distancia sensado: {}".format(d))
#cerrar
sim.simxFinish(self.clientID)
def vision():
while (sim.simxGetConnectionId(clientID) != -1):
err, resolution, image = sim.simxGetVisionSensorImage(
clientID, v1, 0, sim.simx_opmode_buffer)
if err == sim.simx_return_ok:
img = np.array(image, dtype=np.uint8)
img.resize([resolution[1], resolution[0], 3])
# image was originally upside down, turn it 180 degree
img180 = cv2.flip(img, 0)
#convert image from bgr -> rgb
imgfinal = cv2.cvtColor(img180, cv2.COLOR_RGB2BGR)
# show image
cv2.imshow('image', imgfinal)
# press 'q' to exit
if cv2.waitKey(1) & 0xFF == ord('q'):
break
elif err == sim.simx_return_novalue_flag:
print('no image')
pass
else:
print(err)
else:
print("Failed to connect to remote API Server")
sim.simxFinish(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 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): # 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 __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 close(self):
if self.viewer:
self.viewer.close()
self.viewer = None
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
# Now close the connection to CoppeliaSim:
sim.simxFinish(self.clientID)
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 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 close_connection(self):
sim.simxGetPingTime(
self.client_id
) # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive.
sim.simxFinish(
self.client_id) # Now close the connection to CoppeliaSim:
print('Connection closed')
def close_connection(self):
self.actuate([0]*len(self.motors))
# Before closing the connection of CoppeliaSim,
# make sure that the last command sent out had time to arrive.
sim.simxGetPingTime(self.client_id)
sim.simxFinish(self.client_id) # Now close the connection of CoppeliaSim:
print('Connection closed')
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, 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 exit_remote_api_server():
"""
Purpose:
---
This function should wait for the last command sent to arrive at the Coppeliasim server
before closing the connection and then end the communication thread with server
i.e. CoppeliaSim using simxFinish Remote API.
Input Arguments:
---
None
Returns:
---
None
Example call:
---
exit_remote_api_server()
NOTE: This function will be automatically called by test_task_2a executable after ending the simulation.
"""
global client_id
############## ADD YOUR CODE HERE ##############
sim.simxGetPingTime(client_id)
sim.simxFinish(client_id)
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 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 __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 __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 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 __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 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
