def fun_s_and_e(flag):
# print('flag', flag, type(flag))
if flag == '1':
vrep.simxStartSimulation(IDList.clientID.mainID,
vrep.simx_opmode_oneshot)
elif flag == '0':
vrep.simxPauseSimulation(IDList.clientID.mainID,
vrep.simx_opmode_oneshot)
def setAngles(self, angles=[0, 0, 0, 0, 0, 0, 0, 0]):
clientID = self.clientID
h = self.handles
vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
for i in range(8):
err_code = vrep.simxSetJointPosition(clientID, h[i + 1],
angles[i] * np.pi / 180,
vrep.simx_opmode_oneshot)
vrep.simxPauseSimulation(clientID, False)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
return err_code
def pause_simulation(clientID):
result = vrep.simxPauseSimulation(clientID,vrep.simx_opmode_oneshot)
#if result != vrep.simx_return_ok:
# raise Exception('could not pause simulation!')
# Wait two seconds
time.sleep(2)
def pause(self):
vrep.simxPauseSimulation(self.clientID, vrep.simx_opmode_oneshot)
def pause():
""" pause the simulation """
vrep.simxPauseSimulation(clientID, ONESHOT)
time.sleep(0.5)
def pause_simulation(self):
"""Temporary pause simulation. To resume it use start command."""
vrep.simxPauseSimulation(self.client_id, ONE_SHOT_MODE)
def runSimulation():
# Creates map from json file
aMap = Map.Map("P25_X.json", "P25_26_traj.json")
allObstacles = []
# Writes obstacles to file
allObstacles.append(aMap.bounding_polygon)
## for obstacle in aMap.obstacles:
## allObstacles.append(obstacle)
writeToFile(allObstacles, "track.txt")
## rightWall = vrep.simxGetObjectHandle(clientID, "rightWall", vrep.simx_opmode_blocking)
## topWall = vrep.simxGetObjectHandle(clientID, "topWall", vrep.simx_opmode_blocking)
## leftWall = vrep.simxGetObjectHandle(clientID, "leftWall", vrep.simx_opmode_blocking)
## bottomWall = vrep.simxGetObjectHandle(clientID, "bottomWall", vrep.simx_opmode_blocking)
## vrep.simxSetObjectFloatParameter(clientID, rightWall,
# Plans the path
#path = findPathKP(aMap) # Kinematic Point
#path = findPathDP(aMap) # Dynamic Point
#path = findPathDD(aMap) # Differential Drive
## path = findPathKC(aMap) # Kinematic Car
formation = Formation(Map.Map("P26_X.json", "P25_26_traj.json").formation_positions)
paths = formationAcquisition(Map.Map("P26_X.json", "P25_26_traj.json"), formation)
acquisitionLength = len(paths[0])
print(acquisitionLength)
print(len(paths[1]))
paths = formationManeuvering(paths, Map.Map("P26_X.json", "P25_26_traj.json"), formation)
# Reads path from file
#path = readPath("DP_P3.txt")
# Initial values
startNodes = []
gammas = []
for path in paths:
print("hello")
startNodes.append(path[len(path) - 1])
if len(path[len(path) - 1].vel) == 0:
gammas.append(path[len(path) - 1].orientation)
else:
gammas.append(velToAng(path[len(path) - 1].vel))
posZ = 0.025
# ---------------------- Connects to V-rep
# Close any open connections
vrep.simxFinish(-1)
# Connect to the V-REP continuous server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)
if clientID != -1: # if we connected successfully
print('Connected to remote API server')
# --------------------- Setup the simulation
vrep.simxSynchronous(clientID, True)
objectNames = []
numberOfAgents = 10
objectHandles = [None] * numberOfAgents
for i in range(numberOfAgents):
objectNames.append("bubbleRob" + str(i))
# get the handle for the bubbleRobs
print(objectHandles[i])
print(startNodes)
_, objectHandles[i] = vrep.simxGetObjectHandle(clientID, objectNames[i], vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID, objectHandles[i], -1, [startNodes[i].x/10, startNodes[i].y/10, posZ], vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectOrientation(clientID, objectHandles[i], -1, [0, 0, gammas[i]],vrep.simx_opmode_oneshot)
leaderName = "bubbleRob"
_, leaderHandle = vrep.simxGetObjectHandle(clientID, leaderName, vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID, leaderHandle, -1, [aMap.traj_x[0]/10, aMap.traj_y[0]/10, posZ], vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectOrientation(clientID, objectHandles[i], -1, [0, 0, aMap.traj_theta[0]],vrep.simx_opmode_oneshot)
dt = 1
vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, dt, vrep.simx_opmode_oneshot)
# --------------------- Start the simulation
# start our simulation in lockstep with our code
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
count = 0
while count < len(paths[0]): # run for 1 simulated second
if count >= acquisitionLength:
gamma = aMap.traj_theta[count - acquisitionLength]
_ = vrep.simxSetObjectPosition(clientID, leaderHandle, -1, [aMap.traj_x[count - acquisitionLength]/10, aMap.traj_y[count - acquisitionLength]/10, posZ], vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectOrientation(clientID, leaderHandle, -1, [0, 0, gamma], vrep.simx_opmode_oneshot)
for pathNo in range(len(paths)):
currentNode = paths[pathNo][count]
if len(currentNode.vel) == 0:
gamma = currentNode.orientation
else:
gamma = velToAng(currentNode.vel)
_ = vrep.simxSetObjectPosition(clientID, objectHandles[pathNo], -1, [currentNode.x/10, currentNode.y/10, posZ], vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectOrientation(clientID, objectHandles[pathNo], -1, [0, 0, gamma], vrep.simx_opmode_oneshot)
# move simulation ahead one time step
vrep.simxSynchronousTrigger(clientID)
count += dt
# Pause simulation
vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(3)
# Stop simulation
vrep.simxStopSimulation(clientID,
vrep.simx_opmode_blocking)
# Close the connection to V-REP:
vrep.simxFinish(clientID)
def pause_simulation(self):
vrep.simxPauseSimulation(self.client_id, vrep.simx_opmode_blocking)
print(position)
print(orientation)
while True:
char = getch()
if (char == "m"):
print("Stop!")
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
if (char == "s"):
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
exit(0)
if (char == "p"):
vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot_wait)
x_req = 1.35 # to be replaced with robot workspace coordinates
res1, obj1 = vrep.simxGetObjectHandle(clientID, 'Sphere', vrep.simx_opmode_blocking)
if (char == "n"):
pose_list = []
xx = []
zz = []
z_vel, x_vel = input("velocity in Z and X: ")
time1 = input("Time of flight: ")
vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_z, z_vel, vrep.simx_opmode_blocking)
# Connect to the V-REP continuous server
CLIENTID = remote_api.simxStart('127.0.0.1', 19997, True, True, 500, 5)
if CLIENTID != -1: # if we connected successfully
print('Connected to remote API server')
remote_api.simxSynchronous(CLIENTID, True)
dt = 0.1
#remote_api.simxSetFloatingParameter(CLIENTID,
# remote_api.sim_floatparam_simulation_time_step,
# dt, # specify a simulation time step
# remote_api.simx_opmode_oneshot)
remote_api.simxStartSimulation(CLIENTID, remote_api.simx_opmode_oneshot)
add_cube("Kub", [0, 0, 0.025], [1, 1, 1], 5)
print(
remote_api.simxGetObjectHandle(CLIENTID, "Cuboid",
remote_api.simx_opmode_blocking))
remote_api.simxPauseSimulation(CLIENTID, remote_api.simx_opmode_blocking)
print("Somnar")
time.sleep(5)
print("vaknar")
remote_api.simxStopSimulation(CLIENTID, remote_api.simx_opmode_blocking)
# Now close the connection to V-REP:
remote_api.simxFinish(CLIENTID)
def pause_simulation(self):
status = vrep.simxPauseSimulation(self.client_id,
vrep.simx_opmode_oneshot)
time.sleep(0.5)
def __del__(self):
vrep.simxPauseSimulation(self.clientID, vrep.simx_opmode_blocking)
time.sleep(.05)
vrep.simxFinish(self.clientID)
def runSimulation():
# Creates map from json file
aMap = Map("P3.json")
allObstacles = []
# Writes obstacles to file
allObstacles.append(aMap.bounding_polygon)
for obstacle in aMap.obstacles:
allObstacles.append(obstacle)
writeToFile(allObstacles, "track.txt")
# Plans the path
#path = findPathKP(aMap) # Kinematic Point
#path = findPathDP(aMap) # Dynamic Point
#path = findPathDD(aMap) # Differential Drive
path = findPathKC(aMap) # Kinematic Car
# Reads path from file
#path = readPath("DP_P3.txt")
# Initial values
startNode = path[0]
if len(startNode.vel) == 0:
gamma = startNode.orientation
else:
gamma = velToAng(startNode.vel)
posZ = 0.025
# ---------------------- Connects to V-rep
# Close any open connections
vrep.simxFinish(-1)
# Connect to the V-REP continuous server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)
if clientID != -1: # if we connected successfully
print('Connected to remote API server')
# --------------------- Setup the simulation
vrep.simxSynchronous(clientID, True)
objectName = "bubbleRob"
# get the handle for bubbleRob
_, objectHandle = vrep.simxGetObjectHandle(clientID, objectName,
vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID, objectHandle, -1,
[startNode.x / 10, startNode.y / 10, posZ],
vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectOrientation(clientID, objectHandle, -1,
[0, 0, gamma], vrep.simx_opmode_oneshot)
dt = 1
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step, dt,
vrep.simx_opmode_oneshot)
# --------------------- Start the simulation
# start our simulation in lockstep with our code
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
count = 0
while count < len(path): # run for 1 simulated second
currentNode = path[count]
if len(currentNode.vel) == 0:
gamma = currentNode.orientation
else:
gamma = velToAng(currentNode.vel)
_ = vrep.simxSetObjectPosition(
clientID, objectHandle, -1,
[currentNode.x / 10, currentNode.y / 10, posZ],
vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectOrientation(clientID, objectHandle, -1,
[0, 0, gamma],
vrep.simx_opmode_oneshot)
# move simulation ahead one time step
vrep.simxSynchronousTrigger(clientID)
count += dt
# Pause simulation
vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(3)
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Close the connection to V-REP:
vrep.simxFinish(clientID)
def runSimulation():
aMap = Map("P3.json")
allObstacles = []
allObstacles.append(aMap.bounding_polygon)
for obstacle in aMap.obstacles:
allObstacles.append(obstacle)
writeToFile(allObstacles, "track.txt")
# close any open connections
vrep.simxFinish(-1)
# Connect to the V-REP continuous server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)
if clientID != -1: # if we connected successfully
print('Connected to remote API server')
# --------------------- Setup the simulation
vrep.simxSynchronous(clientID, True)
objectName = "bubbleRob"
joint_names = ['bubbleRob_rightMotor', 'bubbleRob_leftMotor']
# joint target velocities discussed below
joint_target_velocities = np.ones(len(joint_names)) * 0.0
# get the handles for each joint and set up streaming
joint_handles = [
vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking)[1]
for name in joint_names
]
_, objectHandle = vrep.simxGetObjectHandle(clientID, objectName,
vrep.simx_opmode_blocking)
print(objectName)
print(objectHandle)
posZ = 0.025
#errorCode = vrep.simxSetObjectPosition(clientID, objectHandle, -1, [posX, posY, posZ], vrep.simx_opmode_oneshot)
#orientation = vrep.simxGetObjectOrientation(clientID, objectHandle, -1, vrep.simx_opmode_streaming)
#print(errorCode)
#print(orientation)
path = findPathDP("P3.json")
print(len(path))
startNode = path[0]
startVel = startNode.vel
gamma = velToAng(startVel)
#print("gamma")
#print(gamma)
errorCode = vrep.simxSetObjectPosition(
clientID, objectHandle, -1, [startNode.x / 10, startNode.y / 10, posZ],
vrep.simx_opmode_oneshot)
errorCode = vrep.simxSetObjectOrientation(clientID, objectHandle, -1,
[0, 0, gamma],
vrep.simx_opmode_oneshot)
#errorCode = vrep.simxSetObjectPosition(clientID, objectHandle, -1, [path[len(path)-1].x/10, path[len(path)-1].y/10, posZ], vrep.simx_opmode_oneshot)
# get handle for target and set up streaming
_, target_handle = vrep.simxGetObjectHandle(clientID, 'bubbleRob',
vrep.simx_opmode_blocking)
dt = 1
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# --------------------- Start the simulation
# start our simulation in lockstep with our code
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
count = 0
while count < len(path): # run for 1 simulated second
#posX += 0.005
#posY += 0.005
currentNode = path[count]
currenNodeName = path[count].name
gamma = velToAng(currentNode.vel)
#print(currentNode.vel)
#print(gamma)
#print(currentNode.x/100)
errorCode = vrep.simxSetObjectPosition(
clientID, objectHandle, -1,
[currentNode.x / 10, currentNode.y / 10, posZ],
vrep.simx_opmode_oneshot)
errorCode = vrep.simxSetObjectOrientation(clientID, objectHandle, -1,
[0, 0, gamma],
vrep.simx_opmode_oneshot)
# move simulation ahead one time step
vrep.simxSynchronousTrigger(clientID)
count += dt
# Reset the orientation
#errorCode = vrep.simxSetObjectOrientation(clientID, objectHandle, -1, [0, 0, gamma], vrep.simx_opmode_oneshot)
#Pause simulation
vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(5)
# stop our simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
rbts.append(rbt_tmp)
res, pos = vrep.simxGetObjectPosition(clientID, rbts[-1], -1, vrep.simx_opmode_streaming)
time.sleep(0.2)
for rbt in rbts:
res, pos = vrep.simxGetObjectPosition(clientID, rbt, -1, vrep.simx_opmode_buffer)
init_pos.append(pos)
# print pos
# Start running simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(3) # wait 10 seconds
# Pause the simulation to get results
vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
pos = []
for rbt in rbts:
res, pos_tmp = vrep.simxGetObjectPosition(clientID, rbt, -1, vrep.simx_opmode_buffer)
pos.append(pos_tmp)
# print pos_tmp
# Stop running the simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
delta_x = []
delta_y = []
for i in range(len(pos)):
delta_x.append(pos[i][0] - init_pos[i][0])
delta_y.append(pos[i][1] - init_pos[i][1])
def pause_simulation(self):
vrep.unwrap_vrep(
vrep.simxPauseSimulation(self._clientID,
vrep.simx_opmode_blocking))
def pause(self):
v.simxPauseSimulation(self._id, self._def_op_mode)
def pause():
errorCode = vrep.simxPauseSimulation(clientID,
vrep.simx_opmode_oneshot_wait)
def pause_sim(self):
vrep.simxPauseSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
return
