Exemple #1
0
    def restart(self,earlyStop = False):
        if (self.cid != None):
            vrep.simxStopSimulation(self.cid, self.mode())
            vrep.simxSynchronousTrigger(self.cid)
        vrep.simxFinish(-1)  # just in case, close all opened connections
        time.sleep(1)
        self.connect()
        time.sleep(1)

        vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode())
        if earlyStop:
            vrep.simxStopSimulation(self.cid, self.mode())
            vrep.simxSynchronousTrigger(self.cid)
            vrep.simxFinish(-1)  # just in case, close all opened connections
            return
        vrep.simxStartSimulation(self.cid, self.mode())
        self.runtime = 0
        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)

        err, self.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)

        err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming)
        self.getTargetStart()
        for i in range(4):
            self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid,
                                                            'Quadricopter_propeller_respondable' + str(i) + str(1),
                                                            self.mode())
Exemple #2
0
 def close(self, kill=False):
     if self.connected:
         remote_api.simxStopSimulation(self.api_id, remote_api.simx_opmode_oneshot_wait)
         remote_api.simxFinish(self.api_id)
     self.connected = False
     if kill and self.vrep_proc is not None:
         os.killpg(self.vrep_proc.pid, signal.SIGTERM)
Exemple #3
0
 def cleanUp(self):
     print "About to stop simulation connected to self.simulID: ", self.simulID
     vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot)
     vrep.simxSynchronousTrigger(self.simulID)                    
     vrep.simxFinish(self.simulID)
     vrep.simxFinish(-1)
     print "Disconnected from V-REP"
def reset_simulation(clientID):
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
    time.sleep(1) # um pequeno sleep entre o stop e o start
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
    for move in startMoves:
        JointControl(clientID, 0, Body, move)
        time.sleep(0.1)
Exemple #5
0
 def disconnection(self):
     """
     Make disconnection with v-rep simulator
     """
     # stop the simulation:
     vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
     
     # Now close the connection to V-REP:	
     vrep.simxFinish(self.clientID)
Exemple #6
0
 def restart_simulation(self):
     mode = vrep.simx_opmode_oneshot_wait
     assert vrep.simxStopSimulation(self.clientID, mode) == 0, \
                                    "StopSim Error"
     time.sleep(0.1)
     self.start_simulation()
     self.num_sim_steps = 0
def finishSimulation(clientID):
    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
    return error
Exemple #8
0
 def stop( self ):
     """
     Stops the simulation
     """
     err = vrep.simxStopSimulation( self.cid, vrep.simx_opmode_oneshot_wait )
     time.sleep(0.01) # Maybe this will prevent V-REP from crashing as often
     return hasattr(self, 'failed') # Returns true if this is a failed run
Exemple #9
0
 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
     return error
    def __init__(self, n_robots, base_name):
        self.name = "line follower tester"
        self.n_robots = n_robots
        self.base_name = base_name

        self.robot_names = [self.base_name]
        for i in range(self.n_robots-1):
            self.robot_names.append(self.base_name+'#'+str(i))

        # Establish connection to V-REP
        vrep.simxFinish(-1)  # just in case, close all opened connections

        # Connect to the simulation using V-REP's remote API (configured in V-REP, not scene specific)
        # http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm
        self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)

        # Use port 19999 and add simExtRemoteApiStart(19999) to some child-script in your scene for scene specific API
        # (requires the simulation to be running)

        if self.clientID != -1:
            print ('Connected to remote API server')
        else:
            print ('Failed connecting to remote API server')
            sys.exit('Could not connect')

        # Reset running simulation
        vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
        time.sleep(0.2)

        # Get initial robots' positions
        self.robot_handles = []
        self.robot_pos0 = []
        for rbt_name in self.robot_names:
            res, rbt_tmp = vrep.simxGetObjectHandle(self.clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
            self.robot_handles.append(rbt_tmp)
            # Initialize data stream
            vrep.simxGetObjectPosition(self.clientID, self.robot_handles[-1], -1, vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_1', vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_2', vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_3', vrep.simx_opmode_streaming)

        time.sleep(0.2)
        for rbt in self.robot_handles:
            res, pos = vrep.simxGetObjectPosition(self.clientID, rbt, -1, vrep.simx_opmode_buffer)
            self.robot_pos0.append(pos)
Exemple #11
0
 def reset(self):
     """In VREP we reset a simulation by stopping and restarting it"""
     eCode = vrep.simxStopSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)  
     if eCode != 0:
         raise Exception("Could not stop VREP simulation")
     eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)   
     if eCode != 0:
         raise Exception("Could not start VREP simulation")
     vrep.simxSynchronousTrigger(self._VREP_clientId)
Exemple #12
0
def reset_vrep():
    print 'Start to connect vrep'
    # Close eventual old connections
    vrep.simxFinish(-1)
    # Connect to V-REP remote server
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
    opmode = vrep.simx_opmode_oneshot_wait
    # Try to retrieve motors and robot handlers
    # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectHandle
    ret_l, LMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", opmode)
    ret_r, RMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", opmode)
    ret_a, RobotHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx", opmode)
    vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, 0, vrep.simx_opmode_blocking)
    vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, 0, vrep.simx_opmode_blocking)
    time.sleep(1)    
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
    vrep.simxFinish(clientID)
    print 'Connection to vrep reset-ed!'
Exemple #13
0
 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_rob(self):
		"""
			Sets the bubbleRob to his starting position; mind the hack, simulation has to be stopped 
		"""

		##### Set absolute position

		#stop simulation
		vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) 

		#100ms delay, this is a hack because server isn't ready either
		time.sleep(0.3)

		#set on absolute position
		err = vrep.simxSetObjectPosition(self.clientID, self.bubbleRobHandle, -1, self.bubbleRobStartPosition, vrep.simx_opmode_oneshot_wait)

		#start simulation again
		vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)

		time.sleep(0.3)
Exemple #15
0
 def resetSimulation(self):
     returnCode = vrep.simx_return_novalue_flag
     while returnCode!=vrep.simx_return_ok:
         returnCode=vrep.simxStopSimulation(self.__clientID, vrep.simx_opmode_oneshot)
         time.sleep(0.5)
     time.sleep(0.5)
     returnCode = vrep.simx_return_novalue_flag
     while returnCode!=vrep.simx_return_ok:
         returnCode=vrep.simxStartSimulation(self.__clientID, vrep.simx_opmode_oneshot)
         time.sleep(0.5)
     time.sleep(1.0)
def exit_gracefully(signum, frame):

    running = False
    global clientID
    # stop the simulation
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

    # Before closing the connection to V-REP,
    # make sure that the last command sent out had time to arrive.
    vrep.simxGetPingTime(clientID)

    # Now close the connection to V-REP:
    vrep.simxFinish(clientID)
    print('connection closed...')

    original_sigint = signal.getsignal(signal.SIGINT)
    # restore the original signal handler as otherwise evil things will happen
    # in raw_input when CTRL+C is pressed, and our signal handler is not re-entrant
    signal.signal(signal.SIGINT, original_sigint)
    # restore the exit gracefully handler here
    signal.signal(signal.SIGINT, exit_gracefully)
Exemple #17
0
def playing():
    c = 0
    model.load_weights('saved-models-modelWeight_buffer_3100.h5')
    sensor_h = sensorHandles()
    #Reading of all the current sensors
    while (1):
        state = sensorInformation(sensor_h, clientID)
        qVal = model.predict(np.array(state).reshape(1, 5), batch_size=1)
        action = np.argmax(qVal)
        new_state = makeMove(state, action)
        with open('Questions.txt', 'a') as writeFile:
            writeFile.write(str(state) + "," + str(action) + "\n")
        state = new_state

        if min(state) < 0.09:
            #sys.exit()
            vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
            time.sleep(1)
            vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
            c = c + 1
            print(c)
Exemple #18
0
    def stop_simulation(self):
        # issue command to stop simulation
        vrep.simxStopSimulation(self.client_id, vrep.simx_opmode_blocking)
        # stop all streaming
        for handle_idx in range(0, self.joint_count):
            _, _ = vrep.simxGetJointPosition(self.client_id,
                                             self.handles[handle_idx],
                                             vrep.simx_opmode_discontinue)

        for handle_idx in range(0, self.joint_count):
            _, _ = vrep.simxGetObjectFloatParameter(
                self.client_id, self.handles[handle_idx], 2012,
                vrep.simx_opmode_discontinue)

        _, _ = vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1,
                                          vrep.simx_opmode_discontinue)
        _, _ = vrep.simxGetObjectOrientation(self.client_id, self.body_handle,
                                             -1, vrep.simx_opmode_discontinue)
        # Just to make sure this gets executed
        vrep.simxGetPingTime(self.client_id)
        self.logger.info("simulation stopped")
def signal_handler(signal, frame):
	print('You pressed ctrl C. Stopping the simulation...')
	return_code = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot)

	x = np.asarray(x_training_labels)
	np.save("x_data", x)
	y = np.asarray(y_training_labels)
	np.save("y_data", y)

	time.sleep(1)
	vrep.simxFinish(clientID)
	sys.exit(0)
Exemple #20
0
    def run_simulation(self, simTime):

        count = simTime
        p = 0
        while count < 1:  # run for 1 simulated second

            self.q = np.zeros(len(self.joint_handles))
            self.dq = np.zeros(len(self.joint_handles))
            self.pos = np.zeros(len(self.joint_handles))

            print("t= %f" % (count))
            for ii, joint_handle in enumerate(self.joint_handles):

                # get the joint angles
                _, self.q[ii] = vrep.simxGetJointPosition(
                    self.clientID, joint_handle, self.block_mode)
                if _ != 0: raise Exception()

                # get the joint velocityp
                _, self.dq[ii] = vrep.simxGetObjectFloatParameter(
                    self.clientID, joint_handle, 2012,
                    self.block_mode)  # ID for angular velocity of the joint
                if _ != 0: raise Exception()

                vrep.simxSetJointPosition(self.clientID, joint_handle, p,
                                          vrep.simx_opmode_oneshot)

            print(self.joint_handles)
            self.record_demo("Abc.csv")
            print("Joint Positions in ", self.q * (180 / 3.14))
            count = count + self.dt
            print("Time Elapsed in simulation", count)
            p = p + (3.14 / 180) * 5
            print("p is %f" % (p))

            vrep.simxSynchronousTrigger(self.clientID)

        # stop our simulation
        print("simulation shutting down")
        vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking)
    def reset(self):
        vrep.simxStopSimulation(self.clientId, vrep.simx_opmode_oneshot)
        quad_pos = [-0.75, 0.1, 0.5]
        random.seed(time.time())
        ball_pos = [-0.75+random.uniform(-0.05,0.05), 0.1+random.uniform(-0.05,0.05), 1.6]
        quad_target_pos = [-0.75, 0.1, 0.5]
        vrep.simxSetObjectPosition(self.clientId, self.quad_target, -1, quad_target_pos, vrep.simx_opmode_oneshot)
        vrep.simxSetObjectPosition(self.clientId, self.quad, -1, quad_pos, vrep.simx_opmode_oneshot)
        vrep.simxSetObjectPosition(self.clientId, self.ball, -1, ball_pos, vrep.simx_opmode_oneshot)
        
        time.sleep(0.6)
        vrep.simxStartSimulation(self.clientId, vrep.simx_opmode_oneshot)
        
        _, self.ball_position = vrep.simxGetObjectPosition(self.clientId, self.ball, self.quad, vrep.simx_opmode_buffer)
        _, self.quad_position = vrep.simxGetObjectPosition(self.clientId, self.quad, -1, vrep.simx_opmode_buffer)
        _, self.quad_orientation = vrep.simxGetObjectOrientation(self.clientId, self.quad, -1, vrep.simx_opmode_buffer)
        _, self.quad_target_position = vrep.simxGetObjectPosition(self.clientId, self.quad_target, -1, vrep.simx_opmode_buffer)
        self.previous_target_pos = np.asarray(self.quad_target_position)
        self.prev_ball_pos = self.ball_position

        # return np.concatenate((np.asarray(self.quad_position), np.asarray(self.ball_position)),axis=0)
        return np.asarray(self.ball_position+ [self.quad_position[2]]+ self.quad_orientation)
Exemple #22
0
    def start_new_episode(self, episode, reset_positions):
        if reset_positions:
            # Stop the simulation if it is running
            vrep.simxStopSimulation(self.__client_ID, vrep.simx_opmode_blocking)
            time.sleep(2) # One second dela
            self.randomize_positions()
            
            time.sleep(1) # One second delay
            # Start simulation
            vrep.simxStartSimulation(self.__client_ID, vrep.simx_opmode_blocking)
            # Set display to enabled or not
            vrep.simxSetBooleanParameter(self.__client_ID, vrep.sim_boolparam_display_enabled, self.__show_display, vrep.simx_opmode_oneshot)
            # Enable threaded rendering
            vrep.simxSetBooleanParameter(self.__client_ID, vrep.sim_boolparam_threaded_rendering_enabled, 1, vrep.simx_opmode_oneshot)
            
            # Catch error
            error = vrep.simxSynchronous(self.__client_ID, 1)
            assert error == 0, "Could not start simulation in synchronous mode."

        # Reset all agents 
        for agent in self.__agents:
            agent.restart()
Exemple #23
0
 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 resetSimulation(self):
     returnCode = vrep.simx_return_novalue_flag
     while returnCode != vrep.simx_return_ok:
         returnCode = vrep.simxStopSimulation(self.__clientID,
                                              vrep.simx_opmode_oneshot)
         time.sleep(0.5)
     time.sleep(0.5)
     returnCode = vrep.simx_return_novalue_flag
     while returnCode != vrep.simx_return_ok:
         returnCode = vrep.simxStartSimulation(self.__clientID,
                                               vrep.simx_opmode_oneshot)
         time.sleep(0.5)
     time.sleep(1.0)
Exemple #25
0
    def restart(self, earlyStop=False):
        if (self.cid != None):
            vrep.simxStopSimulation(self.cid, self.mode())
            vrep.simxSynchronousTrigger(self.cid)
        vrep.simxFinish(-1)  # just in case, close all opened connections
        time.sleep(1)
        self.connect()
        time.sleep(1)

        vrep.simxLoadScene(
            self.cid,
            '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt',
            0, self.mode())
        if earlyStop:
            vrep.simxStopSimulation(self.cid, self.mode())
            vrep.simxSynchronousTrigger(self.cid)
            vrep.simxFinish(-1)  # just in case, close all opened connections
            return
        vrep.simxStartSimulation(self.cid, self.mode())
        self.runtime = 0
        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)

        err, self.front_camera = vrep.simxGetObjectHandle(
            self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)

        err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter,
                                                   vrep.simx_opmode_streaming)
        self.getTargetStart()
        for i in range(4):
            self.propellerScripts[i] = vrep.simxGetObjectHandle(
                self.cid,
                'Quadricopter_propeller_respondable' + str(i) + str(1),
                self.mode())
def main():
    Larm_theta = []
    Rarm_theta = []
    Larm_flag = False
    Rarm_flag = False

    print "This is a gripping test simulation for the Baxter robot"

    clientID = initialize_sim()
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
    joint_library, Larm_jointsdict, Rarm_jointsdict, body_jointsdict, joint_bodynames, joint_Rarm, joint_Larm = declarejointvar(
        clientID)
    collision_library = getCollisionlib(clientID)

    time.sleep(5)

    # stop simulation
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)

    # Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
    vrep.simxGetPingTime(clientID)

    # Close the connection to V-REP
    vrep.simxFinish(clientID)
Exemple #27
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 stop_simulation(self):
		if not self.sim_running:
			raise RuntimeError('Simulation is not running.')
		
		self.RAPI_rc(vrep.simxStopSimulation(self.cID, vrep.simx_opmode_blocking))
		
		# Checking if the server really stopped
		try:
			while True:
				self.RAPI_rc(vrep.simxGetIntegerSignal(self.cID,'sig_debug',vrep.simx_opmode_blocking))
				e = vrep.simxGetInMessageInfo(self.cID,vrep.simx_headeroffset_server_state)
				still_running = e[1] & 1
				if not still_running:
					break
		except: pass
		self.sim_running = False
Exemple #29
0
	def StopSimulator(self):
		print('Attempting to Stop the Simulator')
		if vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait) != 0:
			print('Could not stop the simulator. You can stop the simulator manually by pressing the Stop button in VREP.')
		else:
			print('Successfully stoped the VREP Simulator.')

		# Stop streaming modes to each object
		vrep.simxGetObjectPosition(self.clientID, self.robotHandle, -1, vrep.simx_opmode_discontinue)
		vrep.simxGetObjectOrientation(self.clientID, self.robotHandle, -1, vrep.simx_opmode_discontinue)
		vrep.simxGetObjectPosition(self.clientID, self.cameraHandle, -1, vrep.simx_opmode_discontinue)
		vrep.simxGetObjectPosition(self.clientID, self.ballHandle, -1, vrep.simx_opmode_discontinue)
		vrep.simxGetObjectPosition(self.clientID, self.blueGoalHandle, -1, vrep.simx_opmode_discontinue)
		vrep.simxGetObjectPosition(self.clientID, self.yellowGoalHandle, -1, vrep.simx_opmode_discontinue)
		for handle in self.obstacleHandles:
			vrep.simxGetObjectPosition(self.clientID, handle, -1, vrep.simx_opmode_discontinue)
Exemple #30
0
    def stop_simulation(self):
        """Stop the simulation"""
        start = time.time()

        if not hasattr(self, 'clientID'):
            raise Exception(
                'The simulation has not been started. Check start_simulation function'
            )

        while time.time() - start < self.timeout:
            response = vrep.simxStopSimulation(self.clientID,
                                               vrep.simx_opmode_blocking)
            if response == 0:
                print('Simulation stopped')
                vrep.simxFinish(-1)
                return True

        raise Exception("Can't stop the simulation")
Exemple #31
0
    def stop_sim(self, verbose=None):
        """Stop V-REP simulation."""
        # If necessary, determine whether messages should be displayed
        if verbose is None:
            verbose = self.verbose

        # Stop V-REP simulation
        if self._client_id is None:
            raise ConnectionError("Could not stop V-REP simulation: not "
                                  "connected to V-REP remote API server.")
        res = vrep.simxStopSimulation(self._client_id,
                                      vrep.simx_opmode_blocking)
        if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag):
            raise ServerError("Could not stop V-REP simulation.")

        # If necessary, display confirmation message
        if verbose:
            print("V-REP simulation stopped at "
                  "{}.".format(time.strftime("%H:%M:%S")))
Exemple #32
0
def vrepSim(clientID, action):
	vrep.simxFinish(-1)
	localhost = "127.0.0.1"
	clientID=vrep.simxStart(localhost,19997,True,True,1000,5)
	if clientID!=-1:
		print('Connected to remote API server')
		if action=="start":
			err = vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
			if (not err):
				print('Sim Started')
		elif action=="stop":
			err = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
			if (not err):
				print('Sim Stopped')		
		print("Disconnected from remote API server")
	else:
		print('Failed connecting to remote API server')
	vrep.simxFinish(clientID)
	return clientID
Exemple #33
0
def main():
    # this is the main script where all the code is executed

    # access all the VREP elements
    vrep.simxFinish(-1)
    # just in case, close all opened connections
    clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)

    # start aconnection
    if clientID != -1:
        print("Connected to remote API server")
    else:
        print("Not connected to remote API server")
        sys.exit("Could not connect")

    _ = vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
    # make bigger the sensor from the parts producer

    #Wait until the objects reach the image sensor then stop!
    # perhaps a loop that asks every x second is the sensor activated or not (read signal)

    time.sleep(10)

    _ = vrep.simxStopSimulation(clientID, simx_opmode_blocking)
Exemple #34
0
                  '| Reward for this step: %f' % r,
                  '| Global steps: %i' % STEP_COUNT,
                  '| Epsilon: %.4f' % epsilon, '| ls: ', osc_ls)

            if isFall:
                print("The robot falls down.")
                break
            if isOut:
                print("The robot is outside")
                break
        with open("performance_data/{}.csv".format(t), 'a') as f:
            x += 2
            f.write(
                str(i_episode) + ',' + str(episode_reward) + ',' + str(x) +
                ',' + str(i_step) + ',' + str(y) + '\n')
        vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
        time.sleep(1)

    vrep.simxFinish(clientID)
    print("Finish.")

# if __name__ == "__main__":
#     for i_episode in range(MAX_EPISODES):
#         vrep.simxSynchronous(clientID, 1)
#         vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
#         #print('#################start##################')

#         print("Now we are starting a new episode.")
#         episode_reward = 0
#         #para_vec_old = para_vec
#         dx = 0
def foo (portNumb, instructions):
	clientID = vrep.simxStart('127.0.0.1', portNumb, True, True, 5000, 5)
	if clientID != -1 :
		print ("se pudo establecer la conexión con la api del simulador")
		
		#Recover handlers for robot parts
		LUM,LLM,RUM,RLM,head = recoverRobotParts(clientID)

		#Set Initial Target Velocity to 0
		LUMSpeed = 0
		LLMSpeed = 0 
		RUMSpeed = 0 
		RLMSpeed = 0
		setVelocity(clientID,LUM,LUMSpeed)
		setVelocity(clientID,LLM,LLMSpeed)
		setVelocity(clientID,RUM,RUMSpeed)
		setVelocity(clientID,RLM,RLMSpeed)
		
		#Set simulation to be Synchonous instead of Asynchronous
		vrep.simxSynchronous(clientID, True)
		
		#Setting Time Step to 50 ms (miliseconds)
		dt = 0.05
		#WARNING!!! - Time step should NEVER be set to custom because it messes up the simulation behavior!!!!
		#vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, dt, vrep.simx_opmode_blocking)

		#Start simulation if it didn't start
		vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)

		#This are for controlling where I'm in the instructions while simulation is running
		secuenceIndex = 0
		runInfo = []
		headSecuenceTrace = []
		lowerSpeed, upperSpeed = 0, 0
		secuenceTimes = []
		for secuence in instructions:
			instructionIndex = 0
			headTrace = []
			extraPoints = 0
			runtime = 0
			for instruction in secuence:
				instructionIndex+=1
				
				moveRobot(clientID,instruction[0], LUM, LLM, RUM, RLM)
				
				#This is what makes the simulation Synchronous
				initialTime = 0.0
				actualTime = initialTime + dt
				runtime += dt
				#Condition to stop simulation
				hasFallen = False
				vrep.simxSynchronousTrigger(clientID)

				#Retrive head position
				headPosition = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_oneshot)
				#headTrace.append((headPosition,runtime))
				while((actualTime - initialTime) < (instruction[1]/10)):
					#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
					#TODO do I still need the extra points for time?
					extraPoints += dt
					#Retrive head position
					headPosition = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_oneshot)
					headTrace.append((headPosition,runtime))
					
					#Verify that the model hasn't fallen
					if(headPosition[0] == 0 and headPosition[1][2]<0.65):
						#print("Posición de la cabeza:", headPosition[1][2])
						#print("tiempo: ", runtime)
						hasFallen = True
						break
				if(hasFallen):
					break
			if (hasFallen):
				print ("Secuence: ", secuenceIndex, " has fallen!!")
			else:
				print("Secuence: ", secuenceIndex, " has finished without falling")
			#print(secuence)
			
			secuenceTimes.append(extraPoints)
		#Here I collect the data for the whole secuence
			#filter not valid positions
			headTrace = list(filter(lambda x: x[0][0] == 0,headTrace))
			#add to whole run trace info
			headSecuenceTrace.append(headTrace)

			fallenFactor = 0
			if hasFallen:
				fallenFactor = -50
			#format: (index, score, headtrace((valid,(x,y,z),time))
			runInfo.append((secuenceIndex, sum(map(lambda x:math.log(1/distancia(x[0][1],puntoMovil(x[1]))),headTrace))+fallenFactor,headTrace))
			print("puntaje obtenido",runInfo[-1][1])
			secuenceIndex+=1
			#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)
		
		sortedScore = sorted(runInfo, key=lambda x:x[1], reverse=True)
		return sortedScore
	else:
		print ("No se pudo establecer conexión con la api del simulador en el puerto: ", portNumb)
		print ("Verificar que el simulador está abierto")
                population.append([awrist, aelbow, ashoulder, rs])
                #print "The evolution individual is [awrist, aelbow, ashoulder, dis]: \n" + str(population)

                # Get the robot orientation after the movement sequence
                oret, robotOrient = vrep.simxGetObjectOrientation(clientID, robotHandle, -1, vrep.simx_opmode_buffer)
                msg = "2w1a orientation: (x = " + str(robotOrient[0]) +\
                      ", y = " + str(robotOrient[1]) +\
                      ", z = " + str(robotOrient[2]) + ")"
                print msg
                logging.info(msg)

                msg = "\n----- next move -----\n"
                print msg
                logging.info(msg)
                
                vrep.simxStopSimulation(clientID, opmode)
                time.sleep(1)

            print "The evolution individual is [awrist, aelbow, ashoulder, dis]: \n" + str(population)
            # evolution for 20 times
            for i in xrange(10):
                
                msg = "\n>>>The " + str(i) + " generations: <<<\n"
                print msg
                logging.info(msg)
                population = evolve(population)
                
                # get the distance again
                for j in range(0, len(population)-1):
                    vrep.simxStartSimulation(clientID, opmode)
                    awrist = population[j][0]
 def stop_world(self):
     vrep.unwrap_vrep(
         vrep.simxStopSimulation(self._clientID, vrep.simx_opmode_blocking))
     self.wait_for_ping()
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)

    # Now close the connection to V-REP:
    vrep.simxFinish(clientID)
else:
    print ('Failed connecting to remote API server')
print ('Program ended')
Exemple #39
0
 def stop(self):
     returnCode = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot)
     self.robot.reset()
     if (returnCode>1):
         print "returnCode: ", returnCode
         raise Exception('Could not stop')
 def clean_exit(self):
     _ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
     vrep.simxFinish(self.clientID)
     print 'Program ended'
     sys.exit(0)
    ax4.matshow(q_table.T, cmap=plt.cm.gray)
    ax4.set_title('Q-table')
    ax4.xaxis.set_ticks_position('bottom')
    ax4.set_xlabel('state')
    ax4.set_ylabel('action')
    plt.tight_layout()
    plt.show()
    plt.draw()


if __name__ == '__main__':
    try:
        client_id
    except NameError:
        client_id = -1
    e = vrep.simxStopSimulation(client_id, vrep.simx_opmode_oneshot_wait)
    vrep.simxFinish(-1)
    client_id = vrep.simxStart('127.0.0.1', 19998, True, True, 5000, 5)
    assert client_id != -1, 'Failed connecting to remote API server'

    # print ping time
    sec, msec = vrep.simxGetPingTime(client_id)
    print "Ping time: %f" % (sec + msec / 1000.0)

    robot = Robot(client_id)
    agent = Agent(robot, alpha=0.1, gamma=0.9, epsilon=0.05, q_init=0.3)

    num_episodes = 500
    len_episode = 100
    return_history = []
    try:
Exemple #42
0
 def stopSimulation(self):
     """
     Stop simulation
     """
     # stop the simulation:
     vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter):
    try:
        import vrep
    except:
        print ('--------------------------------------------------------------')
        print ('"vrep.py" could not be imported. This means very probably that')
        print ('either "vrep.py" or the remoteApi library could not be found.')
        print ('Make sure both are in the same folder as this file,')
        print ('or appropriately adjust the file "vrep.py"')
        print ('--------------------------------------------------------------')
        print ('')
    """ =========================================================== """
    """ ============ Imported Libraries: ============ """    
    import time
    import numpy as np
    from captains_log_v1 import log_data
    import os
    import cost
    import csv
    import matplotlib.pyplot as plt
    from mpl_toolkits.mplot3d import Axes3D
    """ =================================================================== """
    # output limits:
    #min_output=0
    #max_output=8.335
    # program parameters:
    global i
    i = 0
    xe = []
    ye = []
    ze = []
    xs = []
    ys = []
    zs = []
    x_qc = []
    y_qc = []
    z_qc = []
    u = []
    v1 = []
    v2 = []
    v3 = []
    v4 = []
    #global variables:
    cumul=0
    last_e=0
    pAlphaE=0
    pBetaE=0
    psp2=0
    psp1=0
    prevEuler=0
    
    cumulAlpha = 0
    cumulBeta = 0
    
    cumulAlphaPos = 0
    cumulBetaPos = 0
    
    s_r = 0
    
    particlesTargetVelocities=[0,0,0,0]
    #speed weight:
    vParam=-2
    #parameters for vertical control
    Kpv=2
    Kiv=0
    Kdv=2
    #parameters for horizontal control:
    Kph=0.4 # TBD
    Kih=0.1 # TBD
    Kdh=1.5
    Kph_pos1=0.4
    Kih_pos1=0.001
    Kdh_pos1=0.05
    Kph_pos0=0.4
    Kih_pos0=0.001
    Kdh_pos0=0.05
    #parameters for rotational control:
    Kpr=0.05
    Kir=0
    Kdr=0.9
    """ =========================================================== """
    # parameters needed for gradient descent:
    t0 = 0
    tf = 1
    dt = 0.01
    
    sum_h_alpha = []
    sum_h_beta = []
    sum_h_pos0 = []
    sum_h_pos1 = []
    
    last_alpha_angle = 0
    last_alpha_pos = 0
    last_beta_angle = 0
    last_beta_pos = 0
       
    delta_alpha_angle = []
    delta_alpha_pos = []
    delta_beta_angle = []
    delta_beta_pos = []
    
    J_h_alpha = []
    J_h_beta = []
    J_h_pos0 = []
    J_h_pos1 = []
    
    paramX = []
    paramY = []
    
    theta_h_angles = [Kph, Kih, Kdh]
    theta_h_pos = [Kph_pos0, Kih_pos0, Kdh_pos0] # Kph_pos0 == Kph_pos1 ...
    
    gd = 0
    """ =========================================================== """
    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_blocking)
    
        #functional/handle code:
        emptyBuff=bytearray()
        [returnCode,targetObj]=vrep.simxGetObjectHandle(clientID,Quadricopter_target,vrep.simx_opmode_blocking)
        [returnCode,qc_base_handle]=vrep.simxGetObjectHandle(clientID,Quadricopter_base,vrep.simx_opmode_blocking)
        [returnCode,qc_handle]=vrep.simxGetObjectHandle(clientID,Quadricopter,vrep.simx_opmode_blocking)
        
        # main loop:
        while True:
            """ ========== vertical control: ========== """
            [returnCode,targetPos]=vrep.simxGetObjectPosition(clientID,targetObj,-1,vrep.simx_opmode_blocking)
            [returnCode,pos]=vrep.simxGetObjectPosition(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking)
            [returnCode, l, w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle, vrep.simx_opmode_blocking)
            
            e=targetPos[2]-pos[2]
            cumul=cumul+e
            diff_e=e-last_e        
            Pvert=Kpv*e
            Ivert=Kiv*cumul
            Dvert=Kdv*diff_e
            thrust=5.335+Pvert+Ivert+Dvert+l[2]*vParam# get thrust
            last_e=e

            """ =========================================================== """
            """ ========== horizontal control: ========== """
            [returnCode,sp]=vrep.simxGetObjectPosition(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking)
            [rc,rc,vx,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_vx',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
            [rc,rc,vy,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_vy',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
            [rc,rc,rc,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_Object_Matrix',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
            [errorCode,M]=vrep.simxGetStringSignal(clientID,'mtable',vrep.simx_opmode_oneshot_wait);
            if (errorCode==vrep.simx_return_ok):
                m=vrep.simxUnpackFloats(M)
            
            alphaE=vy[2]-m[11]
            cumulAlpha = cumulAlpha + alphaE
            diff_alphaE=alphaE-pAlphaE
            alphaCorr=Kph*alphaE + Kih*cumulAlpha + Kdh*diff_alphaE #alpha correction
            
            betaE=vx[2]-m[11]
            cumulBeta = cumulBeta + betaE
            diff_betaE=betaE-pBetaE
            betaCorr=-Kph*betaE - Kih*cumulBeta - Kdh*diff_betaE #beta correction
            
            
            pAlphaE=alphaE
            pBetaE=betaE
            
            cumulAlphaPos = cumulAlphaPos + sp[1]
            cumulBetaPos = cumulBetaPos + sp[0]
            alphaPos=Kph_pos1*(sp[1])+ Kih_pos1*cumulAlphaPos +Kdh_pos1*(sp[1]-psp2) #alpha position correction
            betaPos=Kph_pos0*(sp[0])+ Kih_pos0*cumulBetaPos + Kdh_pos0*(sp[0]-psp1) #beta position correction
            
            alphaCorr=alphaCorr+alphaPos
            betaCorr=betaCorr-betaPos
            
            psp2=sp[1]
            psp1=sp[0]
            
            delta_alpha_angle.append(alphaCorr - last_alpha_angle)
            delta_beta_angle.append(betaCorr - last_beta_angle)
            delta_alpha_pos.append(alphaPos - last_alpha_pos)
            delta_beta_pos.append(betaPos - last_beta_pos)
            last_alpha_angle = alphaCorr
            last_beta_angle = betaCorr
            last_alpha_pos = alphaPos
            last_beta_pos = betaPos
            
            """ =========================================================== """            
            """ ========== rotational control: ========== """
            [returnCode,euler]=vrep.simxGetObjectOrientation(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking)
            [returnCode,orientation]=vrep.simxGetObjectOrientation(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking)
            
            Prot=Kpr*euler[2]
            Drot=Kdr*(euler[2]-prevEuler)
            s_r = s_r + euler[2]
            rotCorr=Prot+Drot
            prevEuler=euler[2]
            
            """ =========================================================== """
            """ ========== set propeller velocities: ========== """
            propeller1_PTV = thrust*(1-alphaCorr+betaCorr-rotCorr)
            propeller2_PTV = thrust*(1-alphaCorr-betaCorr+rotCorr)
            propeller3_PTV = thrust*(1+alphaCorr-betaCorr-rotCorr)
            propeller4_PTV = thrust*(1+alphaCorr+betaCorr+rotCorr)
            particlesTargetVelocities=[propeller1_PTV, propeller2_PTV, propeller3_PTV, propeller4_PTV]
            
            """ =========================================================== """
            """ ========== set parameters for logging data: ========== """
            C_parameters_vert = [0,0,0]
            C_parameters_horr = [0,0,0,0,0,0,0]
            C_parameters_rot = [0,0,0]
            vert_comp = [0,0,0,0]
            horr_comp = [0,0,0,0,[0,0,0],0,0,0,0,0,0]
            rot_comp = [[0,0,0],0]
            
            C_parameters_vert[0] = Kpv
            C_parameters_vert[1] = Kiv
            C_parameters_vert[2] = Kdv
            
            C_parameters_horr[0] = Kph
            C_parameters_horr[1] = Kih
            C_parameters_horr[2] = Kdh
            C_parameters_horr[3] = Kph_pos0
            C_parameters_horr[4] = Kdh_pos0
            C_parameters_horr[5] = Kph_pos1
            C_parameters_horr[6] = Kdh_pos1
            
            C_parameters_rot[0] = Kpr
            C_parameters_rot[1] = Kir
            C_parameters_rot[2] = Kdr
            
            vert_comp[0] = targetPos
            vert_comp[1] = pos
            vert_comp[2] = e
            vert_comp[3] = thrust
            
            horr_comp[0] = alphaE
            horr_comp[1] = betaE
            horr_comp[2] = cumulAlpha
            horr_comp[3] = cumulBeta
            horr_comp[4] = sp
            horr_comp[5] = cumulAlphaPos
            horr_comp[6] = cumulBetaPos
            horr_comp[7] = alphaCorr
            horr_comp[8] = betaCorr
            horr_comp[9] = vx
            horr_comp[10] = vy
            
            rot_comp[0] = euler
            rot_comp[1] = rotCorr
            
            """ =========================================================== """
            """ ========== PLOTTING: ========== """
            ## PLOTTING:
            ze.append(e) # otstapuvanje od z-oska
            xe.append(sp[0]) # otstapuvanje od x-oska
            ye.append(sp[1]) # otstapuvawe od y-oska
#            xs.append(targetPos[0])
#            ys.append(targetPos[1])
#            zs.append(targetPos[2])
#            x_qc.append(pos[0])
#            y_qc.append(pos[1])
#            z_qc.append(pos[2])
#            fig = plt.figure()
#            ax = fig.add_subplot(111, projection='3d')
#            ax.plot_wireframe(xs,ys,zs,color='blue')
#            ax.plot_wireframe(x_qc,y_qc,z_qc,color='red')
#            ax.set_xlim3d(-2.1,2.1)
#            ax.set_ylim3d(-2.1,2.1)
#            ax.set_zlim3d(0,1.9)
#            plt.show()
            
#            u.append(thrust)
#            v1.append(propeller1_PTV)
#            v2.append(propeller2_PTV)
#            v3.append(propeller3_PTV)
#            v4.append(propeller4_PTV)
#            
            plt.plot(xe,color='blue')
            plt.hold(True)
            plt.plot(ye,color='red')
            plt.hold(True)
            plt.plot(ze,color='green')
            plt.hold(True)
#            plt.plot(v4,color='pink')
#            plt.hold(True)
#            plt.axis([0,25,0,15])
#            plt.show()
#            
#            plt.plot(xe,color='blue')
#            plt.axis([0,100,-4,4])
#            plt.show()
#            plt.plot(ye,color='red')
#            plt.axis([0,100,-4,4])
#            plt.show()
#            plt.plot(ze,color='green')
            plt.axis([0,25,-2,2])
            plt.show()
            
            """ =========================================================== """
            """ ========== Gradient descent: ========== """
            sum_h_alpha.append(cumulAlpha)
            sum_h_beta.append(cumulBeta)
            sum_h_pos1.append(cumulAlphaPos)
            sum_h_pos0.append(cumulBetaPos)
            
            J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt)
            J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt)
            J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt)
            J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt)
            
            paramX.append(targetPos[0])
            paramY.append(targetPos[1])
            
            print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
            print "theta_h_angles : ", theta_h_angles
            print "theta_h_pos : ", theta_h_pos
            
            print "sum_h_alpha : ", sum_h_alpha[gd]
            print "sum_h_beta : ", sum_h_beta[gd]
            print "sum_h_pos1 : ", sum_h_pos1[gd]
            print "sum_h_pos0 : ", sum_h_pos0[gd]
            
            print "J_h_alpha : ", J_h_alpha[gd]
            print "J_h_beta : ", J_h_beta[gd]
            print "J_h_pos1 : ", J_h_pos1[gd]
            print "J_h_pos0 : ", J_h_pos0[gd]
            
            print "paramX : ", paramX[gd]
            print "paramY : ", paramY[gd]
            print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
            gd = gd + 1
            print "gd (iterations) : ", gd
            print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
            
            """ =========================================================== """
            if gd == 400:
                with open('GD_43.csv','wb') as f1:
                    writer=csv.writer(f1)
                    for i in range(0, gd):
                        writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]])
                break
            
            """ =========================================================== """
            """ ========== WRITE TO TEXT: ========== """
            ## WRITE TO TEXT:
            log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i)
            i +=1
            """ =========================================================== """
           # send propeller velocities to output:
            [res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities,[],emptyBuff,vrep.simx_opmode_blocking)    
            vrep.simxSynchronousTrigger(clientID)
            """ =========================================================== """
        # stop the simulation:
        vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
        
        # Now close the connection to V-REP:
        vrep.simxFinish(clientID)
            
    else:
        print ('Failed connecting to remote API server')
        
Exemple #44
0
 def stop_sim(self):
     time.sleep(self.sleep_sec_min)
     vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
Exemple #45
0
 def stopSimulation(self):
     vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
Exemple #46
0
def Stop_VREP(clientID):

    vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
    vrep.simxFinish(clientID)
def main():
    Larm_theta = []
    Rarm_theta = []
    Larm_flag = False
    Rarm_flag = False

    print "This is a path planning simulation for the Baxter robot"

    arm_response1 = raw_input("Would you like to move the left arm? (Y or N)")

    #for user reference
    #joint_upperbound = [97.494, 60, 174.987, 150, 175.25, 120, 175.25]
    #joint_lowerbound = [-97.494, -123, -174.987, -2.864, -175.25, -90, -175.25]

    if arm_response1 == 'Y':
        Larm_flag = True
        print "Input Theta in degrees"
        theta1 = raw_input("Theta1 = ")
        Larm_theta.append(float(theta1))
        theta2 = raw_input("Theta2 = ")
        Larm_theta.append(float(theta2))
        theta3 = raw_input("Theta3 = ")
        Larm_theta.append(float(theta3))
        theta4 = raw_input("Theta4 = ")
        Larm_theta.append(float(theta4))
        theta5 = raw_input("Theta5 = ")
        Larm_theta.append(float(theta5))
        theta6 = raw_input("Theta6 = ")
        Larm_theta.append(float(theta6))
        theta7 = raw_input("Theta7 = ")
        Larm_theta.append(float(theta7))

    arm_response2 = raw_input("Would you like to move the right arm? (Y or N)")
    if arm_response2 == 'Y':
        Rarm_flag = True
        print "Input Theta in degrees"
        theta1 = raw_input("Theta1 = ")
        Rarm_theta.append(float(theta1))
        theta2 = raw_input("Theta2 = ")
        Rarm_theta.append(float(theta2))
        theta3 = raw_input("Theta3 = ")
        Rarm_theta.append(float(theta3))
        theta4 = raw_input("Theta4 = ")
        Rarm_theta.append(float(theta4))
        theta5 = raw_input("Theta5 = ")
        Rarm_theta.append(float(theta5))
        theta6 = raw_input("Theta6 = ")
        Rarm_theta.append(float(theta6))
        theta7 = raw_input("Theta7 = ")
        Rarm_theta.append(float(theta7))

    if Larm_flag == False:
        Larm_theta = [0, 0, 0, 0, 0, 0, 0]

    if Rarm_flag == False:
        Rarm_theta = [0, 0, 0, 0, 0, 0, 0]

    #check if thetas are valid
    Rarm_theta = checkjointThetas(Rarm_theta)
    Larm_theta = checkjointThetas(Larm_theta)

    clientID = initialize_sim()
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
    joint_library, Larm_jointsdict, Rarm_jointsdict, body_jointsdict, joint_bodynames, joint_Rarm, joint_Larm = declarejointvar(
        clientID)
    collision_library = getCollisionlib(clientID)

    #generate path
    #Initial thetas are 0, desired goal thetas are user-input

    #path plan for L_joints
    executemovement(clientID, Larm_jointsdict, joint_Larm, collision_library,
                    Larm_theta, 'L')

    #path plan for R_joints
    #executemovement(clientID, Rarm_jointsdict, joint_Rarm, collision_library, Rarm_theta, 'R')

    # stop simulation
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)

    # Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
    vrep.simxGetPingTime(clientID)

    # Close the connection to V-REP
    vrep.simxFinish(clientID)
def stop():
    errorCode = vrep.simxStopSimulation(clientID,
                                        vrep.simx_opmode_oneshot_wait)
Exemple #49
0
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter,
                  Quadricopter_target2, Quadricopter_base2, Quadricopter2):
    try:
        import vrep
    except:
        print('--------------------------------------------------------------')
        print('"vrep.py" could not be imported. This means very probably that')
        print('either "vrep.py" or the remoteApi library could not be found.')
        print('Make sure both are in the same folder as this file,')
        print('or appropriately adjust the file "vrep.py"')
        print('--------------------------------------------------------------')
        print('')
    """ =========================================================== """
    """ ============ Imported Libraries: ============ """
    import time
    import numpy as np
    from captains_log_v1 import log_data
    import os
    import cost
    import csv
    import matplotlib.pyplot as plt
    from mpl_toolkits.mplot3d import Axes3D
    import main_PID_controller
    """ =================================================================== """
    # output limits:
    #min_output=0
    #max_output=8.335
    # program parameters:
    global i
    i = 0
    xf = []
    xl = []
    yl = []
    yf = []
    zl = []
    zf = []
    vxl = []
    vxf = []
    vyl = []
    vyf = []
    euler_l = []
    euler_f = []
    #    xe = []
    #    ye = []
    ze = []
    #    xs = []
    #    ys = []
    #    zs = []
    x_qc = []
    y_qc = []
    z_qc = []
    x_qc2 = []
    y_qc2 = []
    z_qc2 = []
    #    u = []
    v1 = []
    v2 = []
    v3 = []
    v4 = []
    v12 = []
    v22 = []
    v32 = []
    v42 = []
    #global variables:
    cumul = 0
    last_e = 0
    pAlphaE = 0
    pBetaE = 0
    psp2 = 0
    psp1 = 0
    prevEuler = 0

    cumulAlpha = 0
    cumulBeta = 0

    cumulAlphaPos = 0
    cumulBetaPos = 0

    cumul2 = 0
    last_e2 = 0
    pAlphaE2 = 0
    pBetaE2 = 0
    psp22 = 0
    psp12 = 0
    prevEuler2 = 0

    cumulAlpha2 = 0
    cumulBeta2 = 0

    cumulAlphaPos2 = 0
    cumulBetaPos2 = 0

    cumulPOS = 0
    cumulVY = 0
    cumulVX = 0
    cumulSP0 = 0
    cumulSP1 = 0
    cumulEULER = 0
    cumulPOS2 = 0
    cumulVY2 = 0
    cumulVX2 = 0
    cumulSP02 = 0
    cumulSP12 = 0
    cumulEULER2 = 0

    particlesTargetVelocities = [0, 0, 0, 0]
    particlesTargetVelocities2 = [0, 0, 0, 0]
    #speed weight:
    vParam = -2
    #parameters for vertical control
    Kpv = 2
    Kiv = 0
    Kdv = 2
    #parameters for horizontal control:
    Kph = 0.4  # TBD
    Kih = 0.1  # TBD
    Kdh = 1.5
    Kph_pos1 = 0.4
    Kih_pos1 = 0.002
    Kdh_pos1 = 0.05
    Kph_pos0 = 0.4
    Kih_pos0 = 0.002
    Kdh_pos0 = 0.05
    #parameters for rotational control:
    Kpr = 0.05
    #    Kir=0
    Kdr = 0.9
    """ =========================================================== """
    gd = 0
    """ =========================================================== """
    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_blocking)

        #functional/handle code:
        emptyBuff = bytearray()
        [returnCode,
         targetObj] = vrep.simxGetObjectHandle(clientID, Quadricopter_target,
                                               vrep.simx_opmode_blocking)
        [returnCode,
         qc_base_handle] = vrep.simxGetObjectHandle(clientID,
                                                    Quadricopter_base,
                                                    vrep.simx_opmode_blocking)
        [returnCode,
         qc_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter,
                                               vrep.simx_opmode_blocking)

        [returnCode,
         targetObj2] = vrep.simxGetObjectHandle(clientID, Quadricopter_target2,
                                                vrep.simx_opmode_blocking)
        [returnCode,
         qc_base_handle2] = vrep.simxGetObjectHandle(clientID,
                                                     Quadricopter_base2,
                                                     vrep.simx_opmode_blocking)
        [returnCode,
         qc_handle2] = vrep.simxGetObjectHandle(clientID, Quadricopter2,
                                                vrep.simx_opmode_blocking)
        # main loop:
        while True:
            """ ========== vertical control: ========== """
            [returnCode,
             targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1,
                                                     vrep.simx_opmode_blocking)
            [returnCode,
             pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1,
                                               vrep.simx_opmode_blocking)
            [returnCode, l,
             w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle,
                                             vrep.simx_opmode_blocking)

            [returnCode, targetPos2
             ] = vrep.simxGetObjectPosition(clientID, targetObj2, -1,
                                            vrep.simx_opmode_blocking)
            [returnCode,
             pos2] = vrep.simxGetObjectPosition(clientID, qc_base_handle2, -1,
                                                vrep.simx_opmode_blocking)
            [returnCode, l2,
             w2] = vrep.simxGetObjectVelocity(clientID, qc_base_handle2,
                                              vrep.simx_opmode_blocking)
            """ =========================================================== """
            """ ========== horizontal control: ========== """
            [returnCode,
             sp] = vrep.simxGetObjectPosition(clientID, targetObj,
                                              qc_base_handle,
                                              vrep.simx_opmode_blocking)
            [rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction(
                clientID, Quadricopter, vrep.sim_scripttype_childscript,
                'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
            [rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction(
                clientID, Quadricopter, vrep.sim_scripttype_childscript,
                'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
            [rc, rc, rc, rc,
             rc] = vrep.simxCallScriptFunction(clientID, Quadricopter,
                                               vrep.sim_scripttype_childscript,
                                               'qc_Get_Object_Matrix', [], [],
                                               [], emptyBuff,
                                               vrep.simx_opmode_blocking)
            [errorCode,
             M] = vrep.simxGetStringSignal(clientID, 'mtable',
                                           vrep.simx_opmode_oneshot_wait)
            if (errorCode == vrep.simx_return_ok):
                m = vrep.simxUnpackFloats(M)

            m1 = m[0]
            m2 = m[1]
            vx1 = [vx[0], vx[1], vx[2]]
            vx2 = [vx[3], vx[4], vx[5]]
            vy1 = [vy[0], vy[1], vy[2]]
            vy2 = [vy[3], vy[4], vy[5]]

            [returnCode,
             sp2] = vrep.simxGetObjectPosition(clientID, targetObj2,
                                               qc_base_handle2,
                                               vrep.simx_opmode_blocking)
            """ =========================================================== """
            """ ========== rotational control: ========== """
            [returnCode,
             euler] = vrep.simxGetObjectOrientation(clientID, targetObj,
                                                    qc_base_handle,
                                                    vrep.simx_opmode_blocking)
            [returnCode, orientation
             ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1,
                                               vrep.simx_opmode_blocking)

            [returnCode,
             euler2] = vrep.simxGetObjectOrientation(clientID, targetObj2,
                                                     qc_base_handle2,
                                                     vrep.simx_opmode_blocking)
            [returnCode, orientation2
             ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle2, -1,
                                               vrep.simx_opmode_blocking)
            """ =========================================================== """
            """ ========== set propeller velocities: ========== """
            particlesTargetVelocities, cumul, last_e, cumulAlpha, pAlphaE, cumulBeta, pBetaE, cumulAlphaPos, cumulBetaPos, psp2, psp1, prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1, cumulEULER = main_PID_controller.PID(
                targetPos, pos, cumul, last_e, Kpv, Kiv, Kdv, vParam, l, vy1,
                m1, cumulAlpha, pAlphaE, Kph, Kih, Kdh, vx1, cumulBeta, pBetaE,
                cumulAlphaPos, cumulBetaPos, sp, Kph_pos1, Kih_pos1, Kdh_pos1,
                Kph_pos0, Kih_pos0, Kdh_pos0, psp2, psp1, Kpr, Kdr, euler,
                prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1,
                cumulEULER, vx, vy, pos, sp, euler)
            particlesTargetVelocities2, cumul2, last_e2, cumulAlpha2, pAlphaE2, cumulBeta2, pBetaE2, cumulAlphaPos2, cumulBetaPos2, psp22, psp12, prevEuler2, cumulPOS2, cumulVY2, cumulVX2, cumulSP02, cumulSP12, cumulEULER2 = main_PID_controller.PID(
                targetPos2, pos2, cumul2, last_e2, Kpv, Kiv, Kdv, vParam, l2,
                vy2, m2, cumulAlpha2, pAlphaE2, Kph, Kih, Kdh, vx2, cumulBeta2,
                pBetaE2, cumulAlphaPos2, cumulBetaPos2, sp2, Kph_pos1,
                Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0, psp22, psp12,
                Kpr, Kdr, euler2, prevEuler2, cumulPOS2, cumulVY2, cumulVX2,
                cumulSP02, cumulSP12, cumulEULER2, vx, vy, pos, sp, euler)

            particlesTargetVelocities_s = [
                particlesTargetVelocities[0], particlesTargetVelocities[1],
                particlesTargetVelocities[2], particlesTargetVelocities[3],
                particlesTargetVelocities2[0], particlesTargetVelocities2[1],
                particlesTargetVelocities2[2], particlesTargetVelocities2[3]
            ]
            """ =========================================================== """
            """ =========================================================== """
            """ ========== PLOTTING: ========== """
            ## PLOTTING:
            #            ze.append(targetPos[2]-pos[2]) # otstapuvanje od z-oska
            #            xe.append(sp[0]) # otstapuvanje od x-oska
            #            ye.append(sp[1]) # otstapuvawe od y-oska
            #            xs.append(targetPos[0])
            #            ys.append(targetPos[1])
            #            zs.append(targetPos[2])
            #            x_qc.append(pos[0])
            #            y_qc.append(pos[1])
            #            z_qc.append(pos[2])
            #            x_qc2.append(pos2[0])
            #            y_qc2.append(pos2[1])
            #            z_qc2.append(pos2[2])
            #            fig = plt.figure()
            #            ax = fig.add_subplot(111, projection='3d')
            #            ax.plot_wireframe(x_qc,y_qc,z_qc,color='blue')
            #            ax.plot_wireframe(x_qc2,y_qc2,z_qc2,color='red')
            #            ax.set_xlim3d(-2.1,2.1)
            #            ax.set_ylim3d(-2.1,2.1)
            #            ax.set_zlim3d(0,1.9)
            #            plt.show()

            #            u.append(thrust)
            ''' !!! treba da se nacrta i pozicijata i da se sporedi so i bez consensus !!! '''
            xl.append(pos[0])
            xf.append(pos2[0])
            yl.append(pos[1])
            yf.append(pos2[1])
            zl.append(pos[2])
            zf.append(pos2[2])
            vxl.append(vx1[2])
            vxf.append(vx2[2])
            vyl.append(vy1[2])
            vyf.append(vy2[2])
            euler_l.append(euler[2])
            euler_f.append(euler2[2])
            v1.append(particlesTargetVelocities[0])
            v2.append(particlesTargetVelocities[1])
            v3.append(particlesTargetVelocities[2])
            v4.append(particlesTargetVelocities[3])
            v12.append(particlesTargetVelocities2[0])
            v22.append(particlesTargetVelocities2[1])
            v32.append(particlesTargetVelocities2[2])
            v42.append(particlesTargetVelocities2[3])
            plt.plot(yl, color='blue')
            plt.hold(True)
            plt.plot(yf, color='red')
            #            plt.plot(euler_l,color='blue')
            #            plt.plot(euler_f,color='red')
            #            plt.plot(yl,color='green')
            #            plt.hold(True)
            #            plt.plot(yf,color='pink')
            #            plt.hold(True)
            #            plt.plot(v3,color='green')
            #            plt.hold(True)
            #            plt.plot(v4,color='pink')
            plt.hold(True)
            plt.axis([0, 149, -3, 3])
            plt.show()
            #            print " Zaxis error between drones: ", pos[2]-pos2[2]
            ze.append(pos[2] - pos2[2])
            #            ze.append(abs(euler[2])-abs(euler2[2]))
            plt.plot(ze, color='green')
            plt.axis([0, 149, -3, 3])
            plt.show()
            """ =========================================================== """
            """ ========== Gradient descent: ========== """
            #            sum_h_alpha.append(cumulAlpha)
            #            sum_h_beta.append(cumulBeta)
            #            sum_h_pos1.append(cumulAlphaPos)
            #            sum_h_pos0.append(cumulBetaPos)

            #            J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt)
            #            J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt)
            #            J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt)
            #            J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt)

            #            paramX.append(targetPos[0])
            #            paramY.append(targetPos[1])

            #            print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
            #            print "theta_h_angles : ", theta_h_angles
            #            print "theta_h_pos : ", theta_h_pos
            #
            #            print "sum_h_alpha : ", sum_h_alpha[gd]
            #            print "sum_h_beta : ", sum_h_beta[gd]
            #            print "sum_h_pos1 : ", sum_h_pos1[gd]
            #            print "sum_h_pos0 : ", sum_h_pos0[gd]

            #            print "J_h_alpha : ", J_h_alpha[gd]
            #            print "J_h_beta : ", J_h_beta[gd]
            #            print "J_h_pos1 : ", J_h_pos1[gd]
            #            print "J_h_pos0 : ", J_h_pos0[gd]

            #            print "paramX : ", paramX[gd]
            #            print "paramY : ", paramY[gd]
            #            print "cumulPOS : ", cumulPOS2
            #            print "cumulVY : ", cumulVY2
            #            print "cumulVX : ", cumulVX2
            #            print "cumulSP0 : ", cumulSP02
            #            print "cumulSP1 : ", cumulSP12
            #            print "cumulEULER : ", cumulEULER2
            #            print "cumul : ", cumul
            print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
            gd = gd + 1
            print "gd (iterations) : ", gd
            print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
            """ =========================================================== """
            if gd == 150:
                #                with open('GD_43.csv','wb') as f1:
                #                    writer=csv.writer(f1)
                #                    for i in range(0, gd):
                #                        writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]])
                break

#            """ =========================================================== """
            """ ========== WRITE TO TEXT: ========== """
            ## WRITE TO TEXT:
            #            log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i)
            #            i +=1
            """ =========================================================== """
            # send propeller velocities to output:
            [res, retInts, retFloats, retStrings,
             retBuffer] = vrep.simxCallScriptFunction(
                 clientID, Quadricopter, vrep.sim_scripttype_childscript,
                 'qc_propeller_v', [], particlesTargetVelocities_s, [],
                 emptyBuff, vrep.simx_opmode_blocking)
            #            [res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter2,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities2,[],emptyBuff,vrep.simx_opmode_blocking)
            vrep.simxSynchronousTrigger(clientID)
            """ =========================================================== """
        print " Zaxis error between drones: ", ze
        # stop the simulation:
        vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

        # Now close the connection to V-REP:
        vrep.simxFinish(clientID)

    else:
        print('Failed connecting to remote API server')
Exemple #50
0
                pshoulder = vrep.simxGetJointPosition(clientID, shoulderHandle,
                                                      opmode)
                print("Motors reached positions: " + str(ashoulder) + " " +
                      str(aelbow) + " " + str(awrist))

                # Get the robot position after the movement sequence
                pret, robotPos = vrep.simxGetObjectPosition(
                    clientID, robotHandle, -1, vrep.simx_opmode_buffer)
                print ("2w1a position: (x = " + str(robotPos[0]) +\
                      ", y = " + str(robotPos[1]) + ")")

                # Get the robot orientation after the movement sequence
                oret, robotOrient = vrep.simxGetObjectOrientation(
                    clientID, robotHandle, -1, vrep.simx_opmode_buffer)
                print ("2w1a orientation: (x = " + str(robotOrient[0]) +\
                      ", y = " + str(robotOrient[1]) +\
                      ", z = " + str(robotOrient[2]) + ")")

            # End the simulation, wait to be sure V-REP had the time to stop it entirely
            # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStopSimulation
            vrep.simxStopSimulation(clientID, opmode)
            time.sleep(1)
            print("----- Simulation ended -----")

    # Close the connection to V-REP remote server
    # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxFinish
    vrep.simxFinish(clientID)
else:
    print('Failed connecting to remote API server')
print('End')
Exemple #51
0
def reset_simulation(clientID):
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
    time.sleep(1) # um pequeno sleep entre o stop e o start
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
            printlog('simxSetJointTargetVelocity', returnCode)
        elif c == 'i':
            returnCode = vrep.simxSetJointTargetVelocity(
                clientID, jointHandles[0], vel, vrep.simx_opmode_blocking)
            printlog('simxSetJointTargetVelocity', returnCode)
        elif c == 'p':
            returnCode = vrep.simxSetJointTargetVelocity(
                clientID, jointHandles[0], -vel, vrep.simx_opmode_blocking)
            printlog('simxSetJointTargetVelocity', returnCode)
        elif c == 'o':
            returnCode = vrep.simxSetJointTargetVelocity(
                clientID, jointHandles[0], 0, vrep.simx_opmode_blocking)
            printlog('simxSetJointTargetVelocity', returnCode)
        elif c == 'z':
            # stop simulation
            returnCode = vrep.simxStopSimulation(clientID,
                                                 vrep.simx_opmode_blocking)
            printlog('simxStopSimulation', returnCode)
            running = True
            while running:
                returnCode, ping = vrep.simxGetPingTime(clientID)
                returnCode, serverState = vrep.simxGetInMessageInfo(
                    clientID, vrep.simx_headeroffset_server_state)
                running = serverState
            # Start simulation
            returnCode = vrep.simxStartSimulation(clientID,
                                                  vrep.simx_opmode_blocking)
            printlog('simxStartSimulation', returnCode)

        elif c == 'q':
            break
 def stop(self):
     vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
Exemple #54
0
    sumo = Sumo(agent_1, agent_2)
    # If reaches max time, quit
    sumo_maxtime = 120
    update_freq = 0.05
    max_update_count = int(sumo_maxtime / update_freq)

    winner = -1

    # Run match
    for count in range(max_update_count):
        # Update all agents
        sumo.update()
        # Check winner
        winner = sumo.getWinner()
        if winner != -1:
            if winner == 1:
                print("Robot 1 wins")
            elif winner == 2:
                print("Robot 2 wins")
            break
        # Wait for program to update
        time.sleep(update_freq)

    # Tie
    if winner == -1:
        print("Tie")

    # Stop simulation
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
    vrep.simxFinish(-1)
Exemple #55
0
errorCode, eulerAngles=vrep.simxGetObjectOrientation(clientID,car_handle,-1,vrep.simx_opmode_streaming)

while trainCount < train_frames:

  errorCode, eulerAngles=vrep.simxGetObjectOrientation(clientID,car_handle,-1,vrep.simx_opmode_buffer)
  print (eulerAngles)
  trainCount += 1
  print(trainCount)
  #Collision handler
  errorCode, collisionState1=vrep.simxReadCollision(clientID,collision_handle1,vrep.simx_opmode_buffer)

  print(collisionState1)
  print(maxroute)
  
  if abs(eulerAngles[0])>2:
    returnCode=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot)
    time.sleep(5)
    reward = PUNISH
    flipCount +=1
    returnCode=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)

  if collisionState1 == 1 or min(proximitysensor) <=1:
    #returnCode=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot)
    #clientID = tv.Initialize()
    #returnCode=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
    if maxroute < driveCount and  trainCount > observe:
      maxroute = driveCount
      # Log the car's distance at this T.
      data_collect.append([trainCount, maxroute])
      
    driveCount = 0
Exemple #56
0
 def close(self):
     gym.Env.close(self)
     vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
     os.system("TASKKILL /F /IM vrep.exe")
Exemple #57
0
                vrep.simxSetJointForce(clientID, 
                        joint_handle,
                        abs(u[ii]), # force to apply
                        vrep.simx_opmode_blocking)
                if _ !=0 : raise Exception()

            # move simulation ahead one time step
            vrep.simxSynchronousTrigger(clientID)
            count += dt
    else:
        raise Exception('Failed connecting to remote API server')
except Exception, e:
    print e
finally: 
    # stop the simulation
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

    # Before closing the connection to V-REP, 
    # make sure that the last command sent out had time to arrive. 
    vrep.simxGetPingTime(clientID)

    # Now close the connection to V-REP:
    vrep.simxFinish(clientID)
    print 'connection closed...'

    import matplotlib as mpl
    from mpl_toolkits.mplot3d import Axes3D
    import matplotlib.pyplot as plt

    track_hand = np.array(track_hand)
    track_target = np.array(track_target)
Exemple #58
0
 def __del__(self):
     vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking)
Exemple #59
0
def JointControl(clientID, motionProxy, postureProxy, i, Body):
    # Head
    # velocity_x = vrep.simxGetObjectFloatParameter(clientID,vrep.simxGetObjectHandle(clientID,'NAO#',vrep.simx_opmode_oneshot_wait)[1],11,vrep.simx_opmode_streaming)
    pos = vrep.simxGetObjectPosition(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
    angularPos = vrep.simxGetObjectOrientation(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
    alphaPosRegister = []
    betaPosRegister = []
    gamaPosRegister = []
    j = 0
    li = []
    while vrep.simxGetConnectionId(clientID) != -1:
        for x in range(0, 150):
            commandAngles = motionProxy.getAngles('Body', False)
            j+=1
            pos = vrep.simxGetObjectPosition(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
            angularPos = vrep.simxGetObjectOrientation(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
            alphaPosRegister.append(angularPos[0])
            betaPosRegister.append(angularPos[1])
            gamaPosRegister.append(angularPos[2])

            vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
            # Left Leg
            vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming)
            # Right Leg
            vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming)
            # Left Arm
            vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming)
            # Right Arm
            vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming)
            # Left Fingers
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            # Right Fingers
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
        motionProxy.stopMove()
        postureProxy.stopMove()
        # postureProxy.goToPosture("Stand",0.5)
        vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
        break
    print 'End of simulation'
    print 'Final x pos'
    print pos
    calculos = {}
    calcular(alphaPosRegister,calculos)
    desvpadA = calculos['desvio_padrao']
    calculos = {}
    calcular(betaPosRegister,calculos)
    desvpadB = calculos['desvio_padrao']
    calculos = {}
    calcular(gamaPosRegister,calculos)
    desvpadG = calculos['desvio_padrao']
    return [(10*pos[0] - (desvpadA + desvpadB + desvpadG)/3), pos[0]]