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