Example #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())
    def __init__(self):
        self._pid = [PID(),PID(),PID(),PID(),PID(),PID(),PID(),PID(),PID()]
        self._linearVelocity = [0,0,0,0,0,0,0,0,0]
        self._theta = [0,0,0,0,0,0,0,0,0]
        self._positions =[]
        self._convertdeg2rad = 57.295779578552 
        self._num_steps = 0
        self._timestep = 0.05
        self._arm_joints = [0,0,0,0,0,0,0,0,0]
        self._cube = 0
        
        self._simulator = "VRep"
        self._physics_engine = ""
        self._experiment = ""
        self._current_iteration = 0

        vrep.simxFinish(-1) # just in case, close all opened connections
        self._clientID=vrep.simxStart('127.0.0.1',19997,True,False,5000,5) # Connect to V-REP
        
        vrep.simxLoadScene(self._clientID, "kinova_description/urdf/m1n6s300.ttt", 1, vrep.simx_opmode_blocking)
        
        errorCode,self._arm_joints[0] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_1', vrep.simx_opmode_blocking)
        errorCode,self._arm_joints[1] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_2', vrep.simx_opmode_blocking)
        errorCode,self._arm_joints[2] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_3', vrep.simx_opmode_blocking)
        errorCode,self._arm_joints[3] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_4', vrep.simx_opmode_blocking)
        errorCode,self._arm_joints[4] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_5', vrep.simx_opmode_blocking)
        errorCode,self._arm_joints[5] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_6', vrep.simx_opmode_blocking)
        errorCode,self._arm_joints[6] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_finger_1', vrep.simx_opmode_blocking)
        errorCode,self._arm_joints[7] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_finger_2', vrep.simx_opmode_blocking)
        errorCode,self._arm_joints[8] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_finger_3', vrep.simx_opmode_blocking)
        errorCode,self._cube = vrep.simxGetObjectHandle(self._clientID, 'Cuboid', vrep.simx_opmode_blocking)
Example #3
0
 def _configure_environment(self, scene_path):
     """
     Provides a chance for subclasses to override this method and supply
     a different server configuration. By default, we initialize one
     offense agent against no defenders.
     """
     vrep.simxLoadScene(self.clientID, scene_path, 1,vrep.simx_opmode_blocking)
     self._turn_display(False)
Example #4
0
 def load_scene(self, path):
     """
     Load the vrep scene in the specified path
     """
     if self.scene_loaded:
         raise RuntimeError('Scene is loaded already.')
     vrep.simxLoadScene(self.client_id, path, 0, self.modes['blocking'])
     self.scene_loaded = True
Example #5
0
 def reset(self):
     super(StandingUpEnvironment, self).reset()
     # print('*** Reset ***')
     vrep.simxStopSimulation(self.client_id, self.opmode)
     vrep.simxCloseScene(self.client_id, self.opmode)
     vrep.simxLoadScene(self.client_id, self.model_path, 0, self.opmode)
     vrep.simxSynchronous(self.client_id, True)
     vrep.simxStartSimulation(self.client_id, self.opmode)
     self.bioloid = Bioloid(self.client_id)
Example #6
0
    def __init__(self):
        # ROS Publishers
        fin = rospy.Publisher('/finished', Int8, queue_size=1)
        report_sim = rospy.Publisher('/simulation', String, queue_size=1)
        starting = rospy.Publisher('/starting', Int16, queue_size=1)

        # ROS Subscriber
        rospy.Subscriber("/restart", Int8, self.receiveStatus, queue_size=1)

        # V-REP Client Start
        vrep.simxFinish(-1)  # clean up the previous stuff
        clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
        if clientID == -1:
            print("Could not connect to server")
            sys.exit()

        # EPISODE TRACKING
        counter = 0
        while (counter < episodes):
            print("Episode Number ", counter + 1)
            time.sleep(3)

            # Choose simulation randomly from list
            simulation_index = np.random.choice(range(len(VREP_SCENES)))
            sim_name, sim_path = VREP_SCENES[simulation_index]
            msg = String()
            msg.data = sim_name
            report_sim.publish(msg)
            vrep.simxLoadScene(clientID, sim_path, 0, vrep.simx_opmode_blocking)
            time.sleep(2)

            # Start simulation
            r = 1
            while r != 0:
                r = vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)

            msg = Int16()
            msg.data = counter + 1
            starting.publish(msg)
            self.restart = False

            # Wait for signal to stop
            while not self.restart:
                x = 1 + 1

            # Stop and restart simulation
            vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
            counter += 1

        # Finished all episodes
        time.sleep(2)
        msg = Int8()
        msg.data = 1
        fin.publish(msg)
Example #7
0
    def reset(self):
        # first load up the original scene
        scene_did_load = vrep.simxLoadScene(self._client_id, self._scene_file,
                                            0, vrep.simx_opmode_blocking)
        if scene_did_load != vrep.simx_return_ok:
            raise SimulatorException('Could not load scene')

        self._goal_point = np.array([0.0, 0.0, 0.0])
        self._current_action_step = 0
        self._current_sensor_step = None
        # Tuple of joint handle and current position
        self._joint_positions = [(None, 0)] * len(JOINTS)
        self._distance_vector_to_goal = np.array([0, 0, 0])
        self._proximity_sensor_distances = [sys.maxint
                                            ] * len(PROXIMITY_SENSORS)
        self._is_colliding = False
        self._sensor_data_vector = [0] * Environment.outdim

        # get collision handles, joint handles, etc.
        self._scene_handles = self._load_scene_handles()

        self._generate_goal_position()

        vrep.simxStartSimulation(self._client_id, vrep.simx_opmode_blocking)
        time.sleep(0.1)

        self._start_streaming()

        # sleep after streaming starts for data to roll in
        time.sleep(1)

        # We want to get sensors once so that things like _distance_from_goal are properly initialized
        self.getSensors()
Example #8
0
 def load_scene(self, scene_path):
     if self.scene_loaded:
         raise RuntimeError('Scene is already loaded.')
     self.RAPI_rc(
         vrep.simxLoadScene(self.cID, scene_path, 0,
                            vrep.simx_opmode_blocking))
     self.scene_loaded = True
Example #9
0
    def loadScene(self,
                  path="/home/rik/sudo/ros/catkin_ws/src/vrep_test/scenes/",
                  scene="testScene.ttt"):
        returnCode = vrep.simxLoadScene(self.clientID, path + scene, 0,
                                        vrep.simx_opmode_oneshot_wait)

        if returnCode != 0:
            print 'Could not load scene', returnCode
        else:
            print 'Scene loaded'

        # also get the handles for the joints
        for j in range(1, 7):
            returnCode, handle = vrep.simxGetObjectHandle(
                self.clientID, self.jointName + str(j),
                vrep.simx_opmode_oneshot_wait)
            if returnCode == 0:
                self.jointHandles.append(handle)
            else:
                print 'Could not get jointHandle', returnCode

        # get the handle of the dummy position
        returnCode, self.gripperHandle = vrep.simxGetObjectHandle(
            self.clientID, "EEF", vrep.simx_opmode_oneshot_wait)
        if returnCode == 0:
            pass
        else:
            print 'Could not get EEFhandle', returnCode

        returnCode, self.frameHandle = vrep.simxGetObjectHandle(
            self.clientID, "Frame", vrep.simx_opmode_oneshot_wait)
        if returnCode == 0:
            pass
        else:
            print 'Could not get frameHandle', returnCode
    def __init__(self):

        self.ip = '127.0.0.1'
        self.port = 19997
        self.scene = './simu.ttt'
        self.gain = 2
        self.initial_position = [3,3,to_rad(45)]

        self.r = 0.096 # wheel radius
        self.R = 0.267 # demi-distance entre les r

        print('New pioneer simulation started')
        vrep.simxFinish(-1)
        self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5)

        if self.client_id!=-1:
            print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
            res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
            res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
            res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
            res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)

            self.set_position(self.initial_position)
            vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)

        else:
            print('Unable to connect to %s:%s' % (self.ip, self.port))
    def __init__(self):

        self.ip = '127.0.0.1'
        self.port = 19997
        self.scene = './simu.ttt'
        self.gain = 2
        self.initial_position = [3,3,to_rad(45)]

        self.r = 0.096 # wheel radius
        self.R = 0.267 # demi-distance entre les r

        print('New pioneer simulation started')
        vrep.simxFinish(-1)
        self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5)

        if self.client_id!=-1:
            print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
            res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
            res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
            res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
            res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)

            self.set_position(self.initial_position)
            vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)

        else:
            print('Unable to connect to %s:%s' % (self.ip, self.port))
    def __init__(self, seed, rank, ep_len=64, headless=True):
        super().__init__(rank, headless)

        self.target_norm = self.normalise_target()
        self.np_random = np.random.RandomState()
        self.np_random.seed(seed + rank)
        self.ep_len = ep_len

        return_code = vrep.simxSynchronous(self.cid, enable=True)
        check_for_errors(return_code)

        return_code = vrep.simxLoadScene(self.cid, scene_path, 0, vrep.simx_opmode_blocking)
        check_for_errors(return_code)

        # Get the initial configuration of the robot (needed to later reset the robot's pose)
        self.init_config_tree, _, _, _ = self.call_lua_function('get_configuration_tree')
        _, self.init_joint_angles, _, _ = self.call_lua_function('get_joint_angles')

        for i in range(7):
            return_code, handle = vrep.simxGetObjectHandle(self.cid, 'Sawyer_joint' + str(i + 1),
                                                           vrep.simx_opmode_blocking)
            check_for_errors(return_code)
            self.joint_handles[i] = handle

        return_code, self.end_handle = vrep.simxGetObjectHandle(self.cid,
                "BaxterGripper_centerJoint", vrep.simx_opmode_blocking)
        check_for_errors(return_code)
        _, self.target_handle = vrep.simxGetObjectHandle(self.cid,
                "Cube", vrep.simx_opmode_blocking)

        # Start the simulation (the "Play" button in V-Rep should now be in a "Pressed" state)
        return_code = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_blocking)
        check_for_errors(return_code)
Example #13
0
def initiate():
    global __clientID
    global lidars
    global rotor
    global inst
    global steer_handle
    port_num = 19999
    path_vrep = 'C:\Program Files\V-REP3\V-REP_PRO_EDU\\vrep'
    args = [path_vrep, '-gREMOTEAPISERVERSERVICE_' + str(port_num) + '_FALSE_TRUE']

    try:
        inst = sp.Popen(args)
    except EnvironmentError:
        print('(instance) Error: cannot find executable at', args[0])
        raise

    vrep.simxFinish(-1)
    retries = 0
    while True:
        print('(vrepper)trying to connect to server on port', port_num, 'retry:', retries)
        # vrep.simxFinish(-1) # just in case, close all opened connections
        __clientID = vrep.simxStart(
            '127.0.0.1', port_num,
            waitUntilConnected=True,
            doNotReconnectOnceDisconnected=True,
            timeOutInMs=1000,
            commThreadCycleInMs=0)  # Connect to V-REP

        if __clientID != -1:
            print('(vrepper)Connected to remote API server!')
            break
        else:
            retries += 1
            if retries > 15:
                vrep.end()
                raise RuntimeError('(vrepper)Unable to connect to V-REP after 15 retries.')

    vrep.simxLoadScene(__clientID, 'C:\Program Files\V-REP3\V-REP_PRO_EDU\scenes\\not_pristine.ttt',
                       0,  # assume file is at server side
                       vrep.simx_opmode_blocking)
    # vrep.simxFinish(-1)
    # __clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)  # Connect to V-REP
    vrep.simxSynchronous(__clientID, True)
    lidars = ['TriangleA', 'TriangleB', 'TriangleC']
    lidars = [getHandle(x) for x in lidars]
    rotor = getHandle('motor_joint')
    steer_handle = getHandle('steer_joint')
    def __init__(self,
                 seed,
                 rank,
                 initial_policy=None,
                 ep_len=64,
                 headless=True):
        super().__init__(rank, headless)

        self.target_pos = np.array([0.3, -0.5, 0.025])  # TODO: Obtain
        self.waypoint_pos = np.array([0, -0.5, 0.45])  # TODO: Obtain
        self.target_norm = normalise_coords(self.target_pos)
        self.np_random = np.random.RandomState()
        self.np_random.seed(seed + rank)
        self.ep_len = ep_len
        self.initial_policy = initial_policy

        return_code = vrep.simxSynchronous(self.cid, enable=True)
        check_for_errors(return_code)

        return_code = vrep.simxLoadScene(self.cid, scene_path, 0,
                                         vrep.simx_opmode_blocking)
        check_for_errors(return_code)

        # Get the initial configuration of the robot (needed to later reset the robot's pose)
        self.init_config_tree, _, _, _ = self.call_lua_function(
            'get_configuration_tree')
        _, self.init_joint_angles, _, _ = self.call_lua_function(
            'get_joint_angles')
        self.joint_angles = self.init_joint_angles

        self.joint_handles = np.array([None] * len(self.joint_angles))
        for i in range(7):
            return_code, handle = vrep.simxGetObjectHandle(
                self.cid, 'Sawyer_joint' + str(i + 1),
                vrep.simx_opmode_blocking)
            check_for_errors(return_code)
            self.joint_handles[i] = handle

        return_code, self.end_handle = vrep.simxGetObjectHandle(
            self.cid, "BaxterGripper_centerJoint", vrep.simx_opmode_blocking)
        check_for_errors(return_code)
        self.end_pose = self.get_end_pose()
        _, self.target_handle = vrep.simxGetObjectHandle(
            self.cid, "Cube", vrep.simx_opmode_blocking)
        _, self.wall_handle = vrep.simxGetObjectHandle(
            self.cid, "Wall", vrep.simx_opmode_blocking)
        self.init_wall_pos = vrep.simxGetObjectPosition(
            self.cid, self.wall_handle, -1, vrep.simx_opmode_blocking)[1]
        self.wall_distance = normalise_coords(self.init_wall_pos[0],
                                              lower=0,
                                              upper=1)
        self.init_wall_rot = vrep.simxGetObjectOrientation(
            self.cid, self.wall_handle, -1, vrep.simx_opmode_blocking)[1]
        self.wall_orientation = self.init_wall_rot

        # Start the simulation (the "Play" button in V-Rep should now be in a "Pressed" state)
        return_code = vrep.simxStartSimulation(self.cid,
                                               vrep.simx_opmode_blocking)
        check_for_errors(return_code)
Example #15
0
def loadVREPScene(clientID, path_to_scene):
    
    ret = vrep.simxLoadScene(clientID, 
                             path_to_scene, 
                             False, 
                             blocking)
    if ret != return_ok:
        raise Exception('FAIL: Scene failed to load')
def getRobotPosition(nsecs):
    ''' get the position of the robot after nsecs seconds '''
    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:
        vrep.simxLoadScene(clientID, './EvolMor.ttt', 0,
                           vrep.simx_opmode_blocking)
        print('Connected to remote API server')

        vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)

        retCodeHandle, robotHandle = vrep.simxGetObjectHandle(
            clientID, 'robot_shape', vrep.simx_opmode_blocking)
        checkDataFetch(retCodeHandle, robotHandle, 'robot handle')

        # time.sleep(nsecs)
        try:
            # give the robot time to move
            time.sleep(nsecs)
        except KeyboardInterrupt:
            vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
            # 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)
            # Now close the connection to V-REP:
            vrep.simxFinish(clientID)

        retCodePos, objectPos = vrep.simxGetObjectPosition(
            clientID, robotHandle, -1, vrep.simx_opmode_blocking)
        checkDataFetch(retCodePos, objectPos, 'position')

        vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)

        # 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)
        # Now close the connection to V-REP:
        vrep.simxFinish(clientID)
    else:
        print('Failed connecting to remote API server')
    print('Program ended')

    return objectPos[0]
Example #17
0
    def start_vrep(self):
        self.client_id = vrep.simxStart(self.host, self.port_num, True, True, 5000, 5)
        return_code = vrep.simxSynchronous(self.client_id, enable=True)
        self.check_for_errors(return_code)

        dir_path = os.path.dirname(os.path.realpath(__file__))
        scene_path = dir_path + '/my_scene3.ttt'
        return_code = vrep.simxLoadScene(self.client_id, scene_path, 0, vrep.simx_opmode_blocking)
        self.check_for_errors(return_code)
        return_code = vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
        self.check_for_errors(return_code)
Example #18
0
    def __init__(self):
        print('Program started')
        self.models_path = os.getcwd() + "/../models/"

        vrep.simxFinish(-1)  # just in case, close all opened connections
        self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
                                       5)  # Connect to V-REP
        vrep.simxSynchronous(self.clientID, True)

        self.youbot_cmd = rospy.Publisher('cmd_vel', Twist, queue_size=100)
        if self.clientID != -1:
            print('Connected to remote API server')
            vrep.simxLoadScene(self.clientID,
                               os.getcwd() + '/../scene/remote api.ttt', 0xFF,
                               vrep.simx_opmode_blocking)
            self.tables = []
            self.robot = Robot(self.clientID, self.models_path)
            self.reset = True
            self.planner = Planner()
        else:
            print('Failed connecting to remote API server')
        print('Program ended')
Example #19
0
    def load_scene(self, filename, server_side=True):
        """Load scene from file."""
        SERVER_SIDE = 0x00
        CLIENT_SIDE = 0x01

        if self._client_id is None:
            raise ConnectionError(
                "Could not load scene from file {}: not connected to V-REP "
                "remote API server.".format(filename))
        side = SERVER_SIDE if server_side else CLIENT_SIDE
        res = vrep.simxLoadScene(self._client_id, filename, side,
                                 vrep.simx_opmode_blocking)
        if res != vrep.simx_return_ok:
            raise ServerError("Could not load scene from file {}."
                              "".format(filename))
Example #20
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 __init__(self):

        self.ip = '127.0.0.1'
        self.port = 19997
        self.scene = './simu.ttt'
        self.gain = 2
        self.initial_position = [1, 1, to_rad(45)]

        self.r = 0.096  # wheel radius
        self.R = 0.267  # demi-distance entre les r

        self.activated_sensors = [1, 4, 5, 8, 9, 12, 13, 16]

        #print('New pioneer simulation started')
        vrep.simxFinish(-1)
        self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000,
                                        5)

        if self.client_id != -1:
            #print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
            res = vrep.simxLoadScene(self.client_id, self.scene, 1,
                                     vrep.simx_opmode_oneshot_wait)
            res, self.pioneer = vrep.simxGetObjectHandle(
                self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
            res, self.left_motor = vrep.simxGetObjectHandle(
                self.client_id, 'Pioneer_p3dx_leftMotor',
                vrep.simx_opmode_oneshot_wait)
            res, self.right_motor = vrep.simxGetObjectHandle(
                self.client_id, 'Pioneer_p3dx_rightMotor',
                vrep.simx_opmode_oneshot_wait)
            self.proximity_sensors = [
                0 for i in range(len(self.activated_sensors))
            ]

            for i in range(len(self.activated_sensors)):
                res, self.proximity_sensors[i] = vrep.simxGetObjectHandle(
                    self.client_id, 'Pioneer_p3dx_ultrasonicSensor' +
                    str(self.activated_sensors[i]),
                    vrep.simx_opmode_oneshot_wait)

        else:
            print('Unable to connect to %s:%s' % (self.ip, self.port))
            exit()

        self.set_position2(self.initial_position)
        #self.set_position(self.initial_position)
        #vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
        firt_load_of_sensors = self.load_proximity_sensors()
Example #22
0
 def connectToVREP(self, VREP_World=None):
     "Connect to VREP and load the correct world if needed"
     "FIXME: SHOULD LAUNCH VREP IF NOT RUNNING"
     VREP_exec = 'vrep'
     if self.VREPSimulation == None:
         self.VREPSimulation = VREP_World
     
     '1. check that V-Rep is running and see whether we are already connected to it. Otherwise connect'
     if VREP_exec not in check_output(["ps","-f", "--no-headers",  "ww", "-C", "vrep"]):
         raise Exception(("V-REP is not running! Please start V-REP with scene: %s" % self.VREPSimulation))
     else:
         "Check if we are connected with the passed clientId already"
         if self._VREP_clientId is not None:
             print "ClientId = " ,self._VREP_clientId
             connId = vrep.simxGetConnectionId(self._VREP_clientId)
             print "My connId is " , connId
             if connId == -1:                                 # we are not: set client Id to none and re-connect
                 print "Disconnecting all existing connections to V-REP"
                 vrep.simxFinish(-1)
                 self._VREP_clientId = None            
         while self._VREP_clientId is None:
             self._VREP_clientId = vrep.simxStart(self._host, self._kheperaPort,True,True, 5000,5)
             if not self._VREP_clientId == -1:
                 eCode = vrep.simxSynchronous(self._VREP_clientId, True)
                 if eCode != 0:
                     raise Exception("Failed to connect to VREP simulation. Bailing out")
 #     print " we are connected with clientId ", self._VREP_clientId
     "2. Check the correct world is running"
     if self.VREPSimulation is not None: 
         VREP_Scene = split(self.VREPSimulation)[1]
         if VREP_Scene not in check_output(["ps","-f", "--no-headers",  "ww", "-C", "vrep"]):
             eCode = vrep.simxLoadScene(self._VREP_clientId, self.VREPSimulation, 0, vrep.simx_opmode_oneshot_wait)
             if eCode != 0:
                 raise Exception(("Could not load into V-REP the world",  self.VREPSimulation))     
 
 
     "3. Make sure VREP has bees set to the correct directory for saving data"
     self.setDataDir(self.dataDir)
             
     '4. Start simulation'
     eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
     if eCode != 0:
         raise Exception("VREP simulation cannot get started")
     else:
         print "V-REP simulation is running with clientId: ", self._VREP_clientId
         return self._VREP_clientId 
Example #23
0
    def __init__(self):

        self.ip = '127.0.0.1'
        self.port = 19997
        self.scene = './fred.ttt'
        self.gain = [1, 15]
        self.initial_position = [-5, 0, to_rad(0)]

        self.r = 0.096  # wheel radius
        self.R = 0.267  # demi-distance entre les r

        print('New FRED simulation started')
        vrep.simxFinish(-1)
        self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000,
                                        5)

        if self.client_id != -1:
            print('Connected to remote API server on %s:%s' %
                  (self.ip, self.port))
            res = vrep.simxLoadScene(self.client_id, self.scene, 1,
                                     vrep.simx_opmode_oneshot_wait)
            res, self.fred = vrep.simxGetObjectHandle(
                self.client_id, 'Fred', vrep.simx_opmode_oneshot_wait)
            res, self.tourelle = vrep.simxGetObjectHandle(
                self.client_id, 'Plateau', vrep.simx_opmode_oneshot_wait)
            res, self.back = vrep.simxGetObjectHandle(
                self.client_id, 'Bras_arrire', vrep.simx_opmode_oneshot_wait)
            res, self.direction_motor = vrep.simxGetObjectHandle(
                self.client_id, 'Direction', vrep.simx_opmode_oneshot_wait)
            res, self.traction_motor = vrep.simxGetObjectHandle(
                self.client_id, 'Traction', vrep.simx_opmode_oneshot_wait)

            self.set_position(self.initial_position)
            vrep.simxStartSimulation(self.client_id,
                                     vrep.simx_opmode_oneshot_wait)

            vrep.simxSetJointTargetVelocity(self.client_id,
                                            self.traction_motor, 0,
                                            vrep.simx_opmode_oneshot_wait)
            vrep.simxSetJointTargetVelocity(self.client_id,
                                            self.direction_motor, 0,
                                            vrep.simx_opmode_oneshot_wait)

        else:
            print('Unable to connect to %s:%s' % (self.ip, self.port))
Example #24
0
def Connect_vrep(ip = '127.0.0.1', port = 19997, path = vrep_scenes_path):
    '''输入vrep地址和端口,连接vrep并返回可用的clientID'''
    print('程序开始,准备连接V-rep')
    scene_path = path + 'car_cross_sensor.ttt'
    vrep.simxFinish(-1)
    while True:
        cID = vrep.simxStart(ip, port, True, True, 5000, 5)
        if cID > -1:
            print('连接成功')
            err = vrep.simxLoadScene(cID, scene_path, 0, vrep.simx_opmode_blocking)
            if err == 0:
                print('场景加载完成')
            else:
                print('场景加载失败')
            return cID
        else:
            time.sleep(0.50)
            print('连接失败,尝试重新连接')
    def init_vrep_connection(self):
        vrep.simxFinish(-1) # just in case, close all opened connections
        self.client_id=vrep.simxStart(self.ip,self.port,True,True,5000,5) # Connect to V-REP

        if self.client_id!=-1:
            print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
            res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)

            #get motor handler
            self.motor_handlers = []
            for motor in self.robot.motors:
                res, motor_handler = vrep.simxGetObjectHandle(
                    self.client_id,
                    motor.name,
                    vrep.simx_opmode_oneshot_wait
                )
                self.motor_handlers.append(motor_handler)
            self.motor_handlers.sort()
            print(self.motor_handlers)

            #get effector handler
            res, self.effector_handler = vrep.simxGetObjectHandle(
                self.client_id,
                'effector',
                vrep.simx_opmode_oneshot_wait
            )

            #get collision handler
            self.collision_handlers = []
            for collision in self.collision_list:
                res, collision_handler = vrep.simxGetObjectHandle(
                    self.client_id,
                    collision,
                    vrep.simx_opmode_oneshot_wait
                )
                self.collision_handlers.append(collision_handler)

            vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
    def __init__(self, scene_name, rank, headless):
        # Launch a V-Rep server
        # Read more here: http://www.coppeliarobotics.com/helpFiles/en/commandLine.htm
        port_num = base_port_num + rank
        remote_api_string = '-gREMOTEAPISERVERSERVICE_' + str(port_num) + '_FALSE_TRUE'
        args = [*xvfb_args, vrep_path, '-h' if headless else '', remote_api_string]
        self.cid = -1
        while self.cid == -1:
            self.process = Popen(args, preexec_fn=os.setsid, stdout=DEVNULL)
            time.sleep(6)
            self.cid = vrep.simxStart(host, port_num, True, True, 5000, 5)
            if self.cid == -1:
                print(f"{rank} failed to connect to V-REP. Retrying...")
                try:
                    pgrp = os.getpgid(self.process.pid)
                    os.killpg(pgrp, signal.SIGKILL)
                except ProcessLookupError:
                    pass
        catch_errors(vrep.simxSynchronous(self.cid, enable=True))

        scene_path = os.path.join(scene_dir_path, f'{scene_name}.ttt')
        catch_errors(vrep.simxLoadScene(self.cid, scene_path, 0, vrep.simx_opmode_blocking))
        atexit.register(self.close)
Example #27
0
 def loadscn(self, clientID, model):
     error=vrep.simxLoadScene(clientID, model, 2, vrep.simx_opmode_oneshot_wait)
     return error
Example #28
0
 def load_scene(self, scene):
     """Load existing scene by it's name"""
     return vrep.simxLoadScene(self.client_id, SCENES_DIR + scene, 0xFF,
                               BLOCKING_MODE)
Example #29
0
nino_ttt=cwd+"/nino.ttt"
print(nino_ttt)

# 
# cmd_line = "sudo ./vrep/vrep.sh"
# p = subprocess.Popen(cmd_line, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
# out = p.communicate()[0]
# print out

# time.sleep(1)

vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)

if clientID != -1:
    res = vrep.simxLoadScene(clientID, nino_ttt, 0xFF, vrep.simx_opmode_oneshot)
    returnCode=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
    print("connection successful!!")
    #time.sleep(2)


    while True:
        try:
            pass
        except KeyboardInterrupt:
            returnCode=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot)
            raise

else:
     print("connection failed!!")
     sys.exit()
Example #30
0
 def load_scene(self, scene_name):
     vrep.simxLoadScene(self.clientID, scene_name + ".ttt", 0xFF,
                        vrep.simx_opmode_blocking)
     return
Example #31
0
    def reset(self):
        vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
        time.sleep(0.1)

        # Setup V-REP simulation
        print("Setting simulator to async mode...")
        vrep.simxSynchronous(self.clientID, True)
        vrep.simxSetFloatingParameter(
            self.clientID,
            vrep.sim_floatparam_simulation_time_step,
            self.dt,  # specify a simulation time step
            vrep.simx_opmode_oneshot)

        # Load V-REP scene
        print("Loading scene...")
        vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF,
                           vrep.simx_opmode_blocking)

        # Get quadrotor handle
        err, self.quad_handle = vrep.simxGetObjectHandle(
            self.clientID, self.quad_name, vrep.simx_opmode_blocking)

        # Initialize quadrotor position and orientation
        vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1,
                                   vrep.simx_opmode_streaming)
        vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1,
                                      vrep.simx_opmode_streaming)
        vrep.simxGetObjectVelocity(self.clientID, self.quad_handle,
                                   vrep.simx_opmode_streaming)

        # Start simulation
        vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)

        # Initialize rotors
        print("Initializing propellers...")
        for i in range(len(self.propellers)):
            vrep.simxClearFloatSignal(self.clientID, self.propellers[i],
                                      vrep.simx_opmode_oneshot)

        # Get quadrotor initial position and orientation
        err, self.pos_start = vrep.simxGetObjectPosition(
            self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
        err, self.euler_start = vrep.simxGetObjectOrientation(
            self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
        err, self.vel_start, self.angvel_start = vrep.simxGetObjectVelocity(
            self.clientID, self.quad_handle, vrep.simx_opmode_buffer)

        self.pos_start = np.asarray(self.pos_start)
        self.euler_start = np.asarray(self.euler_start) * 10.
        self.vel_start = np.asarray(self.vel_start)
        self.angvel_start = np.asarray(self.angvel_start)

        self.pos_old = self.pos_start
        self.euler_old = self.euler_start
        self.vel_old = self.vel_start
        self.angvel_old = self.angvel_start
        self.pos_new = self.pos_old
        self.euler_new = self.euler_old
        self.vel_new = self.vel_old
        self.angvel_new = self.angvel_old

        self.pos_error = (self.pos_start + self.setpoint_delta[0:3]) \
                         - self.pos_new
        self.euler_error = (self.euler_start + self.setpoint_delta[3:6]) \
                           - self.euler_new
        self.vel_error = (self.vel_start + self.setpoint_delta[6:9]) \
                         - self.vel_new
        self.angvel_error = (self.angvel_start + self.setpoint_delta[9:12]) \
                            - self.angvel_new

        self.pos_error_all = self.pos_error
        self.euler_error_all = self.euler_error

        self.init_f = 5.8

        self.propeller_vels = [
            self.init_f, self.init_f, self.init_f, self.init_f
        ]

        self.timestep = 1

        return self.get_state()
Example #32
0
    def __init__(self, episode_len=None):

        self.clientID = None
        # Start V-REP connection
        try:
            vrep.simxFinish(-1)
            print("Connecting to simulator...")
            self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
                                           5000, 5)
            if self.clientID == -1:
                print("Failed to connect to remote API Server")
                self.vrep_exit()
        except KeyboardInterrupt:
            self.vrep_exit()
            return

        self.episode_len = episode_len
        self.timestep = 0
        self.dt = .001
        self.propellers = [
            'rotor1thrust', 'rotor2thrust', 'rotor3thrust', 'rotor4thrust'
        ]
        self.quad_name = 'Quadricopter'
        self.scene_name = 'quad_innerloop.ttt'
        self.setpoint_delta = np.array(
            [0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.])
        self.quad_handle = None

        self.pos_start = np.zeros(3)
        self.euler_start = np.zeros(3)
        self.vel_start = np.zeros(3)
        self.angvel_start = np.zeros(3)
        self.pos_old = self.pos_start
        self.euler_old = self.euler_start
        self.vel_old = self.vel_start
        self.angvel_old = self.angvel_start
        self.pos_new = self.pos_old
        self.euler_new = self.euler_old
        self.vel_new = self.vel_old
        self.angvel_new = self.angvel_old

        ob_high = np.array([
            10., 10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21., 10.,
            10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21.
        ])
        ob_low = -ob_high
        self.observation_space = Box(low=ob_low,
                                     high=ob_high,
                                     dtype=np.float32)
        ac_high = np.array([20., 20., 20., 20.])
        ac_low = np.zeros(4)
        self.action_space = Box(low=ac_low, high=ac_high, dtype=np.float32)

        print("Setting simulator to async mode...")
        vrep.simxSynchronous(self.clientID, True)
        vrep.simxSetFloatingParameter(
            self.clientID,
            vrep.sim_floatparam_simulation_time_step,
            self.dt,  # specify a simulation time step
            vrep.simx_opmode_oneshot)

        # Load V-REP scene
        print("Loading scene...")
        vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF,
                           vrep.simx_opmode_blocking)

        # Get quadrotor handle
        err, self.quad_handle = vrep.simxGetObjectHandle(
            self.clientID, self.quad_name, vrep.simx_opmode_blocking)

        # Initialize quadrotor position and orientation
        vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1,
                                   vrep.simx_opmode_streaming)
        vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1,
                                      vrep.simx_opmode_streaming)
        vrep.simxGetObjectVelocity(self.clientID, self.quad_handle,
                                   vrep.simx_opmode_streaming)
Example #33
0
#and then three rings at centred .25d, .5d, .75d with radius = 2


k = functions.keys()
k.sort()

iter = 1
for key in k:
    state_file = open('Trajectories/{}.txt'.format(key+"state"),'w+')
    action_file = open('Trajectories/{}.txt'.format(key+"action"),'w+')

    vrep.simxFinish(-1)  # just in case, close all opened connections
    time.sleep(1)
    cid = connect()
    time.sleep(1)
    vrep.simxLoadScene(cid,'/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt',0,mode())
    vrep.simxStartSimulation(cid,mode())
    runtime = 0

    err, copter = vrep.simxGetObjectHandle(cid, "Quadricopter_base",
                                           vrep.simx_opmode_oneshot_wait)

    err, target = vrep.simxGetObjectHandle(cid, "Quadricopter_target",
                                           vrep.simx_opmode_oneshot_wait)

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

    err, lin, ang = vrep.simxGetObjectVelocity(cid, copter, vrep.simx_opmode_streaming)
    err, orgpos = getTargetStart(cid,target)

    real_args(args,orgpos )
Example #34
0
 def loadscn(self, clientID, model):
     time1 = time()
     error = vrep.simxLoadScene(clientID, model, 2, vrep.simx_opmode_oneshot_wait)
     time2 = time()
     print "LOGGING time for loading VREP scene in seconds: ", time2 - time1
     return error
Example #35
0
R = 0.267 # demi-distance entre les r

# neural network config
ni = 3 # number of input nodes (without bias)
nj = 10 # number of hidden nodes
nk = 2 # number of output nodes

alpha = [1/4,1/4,1/(math.pi)]

print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
client_id=vrep.simxStart(ip,port,True,True,5000,5) # Connect to V-REP

if client_id!=-1:
    print ('Connected to remote API server on %s:%s' % (ip, port))
    res = vrep.simxLoadScene(client_id, scene, 1, vrep.simx_opmode_oneshot_wait)
    res, pioneer = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
    res, left_motor = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
    res, right_motor = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)

    # set initial position, -1 is absolute
    vrep.simxSetObjectPosition(client_id, pioneer, -1, [position_init[0], position_init[1], 0.13879], vrep.simx_opmode_oneshot_wait)
    vrep.simxSetObjectOrientation(client_id, pioneer, -1, [0, 0, to_deg(position_init[2])], vrep.simx_opmode_oneshot_wait)


    network = NN(ni,nj,nk)

    vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait)

    position = position_init # [x,y,theta]
    network_input = [0, 0, 0]
Example #36
0
    clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
    if clientID == -1:
        print("Failed to connect to remote API Server")
        vrep_exit(clientID)
    # Setup V-REP simulation
    print("Setting simulator to async mode...")
    vrep.simxSynchronous(clientID, True)
    dt = .001
    vrep.simxSetFloatingParameter(clientID,
                                  vrep.sim_floatparam_simulation_time_step,
                                  dt,  # specify a simulation time step
                                  vrep.simx_opmode_oneshot)

    # Load V-REP scene
    print("Loading scene...")
    vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
    # Initialize rotors
    print("Initializing propellers...")
    for i in range(len(propellers)):
        vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
except KeyboardInterrupt:
    vrep_exit(clientID)

err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)

# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming)


def reset():
vrepProcess = subprocess.Popen(vrep_cmd, shell=False, preexec_fn=os.setsid)

# connect to V-Rep Remote Api Server
vrep.simxFinish(-1)  # close all opened connections
clientID = vrep.simxStart('127.0.0.1', portNb, True, False, 5000,
                          5)  # Connect to V-REP

if clientID == -1:
    print('Failed connecting to remote API server')
else:
    print('Connected to remote API server')
    # load scene
    time.sleep(5)  # to avoid errors
    scenePath = 'MicoRobotTest.ttt'
    returnCode = vrep.simxLoadScene(
        clientID, scenePath, 1, vrep.simx_opmode_oneshot_wait
    )  # vrep.simx_opmode_blocking is recommended
    printlog('simxLoadScene', returnCode)

    # when using child script
    # if len(sys.argv) >= 10:
    #     portNb = int(sys.argv[1])
    #     jointHandles = list(map(int,sys.argv[2:8]))
    #     fingersH1 = int(sys.argv[8])
    #     fingersH2 = int(sys.argv[9])
    # else:
    #     print("Indicate following arguments: 'portNumber jointHandles'")
    #     time.sleep(5.0)
    #     sys.exit(0)

    # get Handles
def loadscn(clientID, model=genetic_bioloid):
    error=vrep.simxLoadScene(clientID, model, 2, vrep.simx_opmode_oneshot_wait)
    return error