def getMapa(resolucion=0.02): try: vrep_host = '127.0.0.1' vrep_port = 19997 scene = None start = False print('Obteniendo Mapa...') io = VrepIO(vrep_host, vrep_port, scene, start) io.restart_simulation() gmapa = MapBuilder(io, resolucion) io.stop_simulation() io.close() print('El mapa se ha construido con exito') return gmapa except: print( "Error el la conexion con el simulador, ejecute este fragmento de nuevo" ) print('Error al obtener mapa') return None
class Simulator(Observable): def __init__(self, vrep_host='127.0.0.1', vrep_port=19997, scene=None, start=False): self.io = VrepIO(vrep_host, vrep_port, scene, start) # 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 self.robots = [] self.vrep_mode = vrep_mode self.n_robots = 0 self.suffix_to_epuck = {} self.seconds_to_run = 0 self._condition = threading.Condition() self.routine_manager = RoutineManager() self.eatable_objects = [] self.logger = Logger() Observable.__init__(self) def wait(self, seconds): start = self.io.get_simulation_current_time() while self.io.get_simulation_current_time() - start < seconds: sleep(0.005) def close(self): self.detach_all_routines() sleep(0.3) self.io.stop_simulation() self.io.close() def get_object_position(self, object_name): return array(self.io.get_object_position(object_name)) def set_object_position(self, object_name, position): return self.io.set_object_position(object_name, position) def add_sphere(self, position, sizes=[0.1, 0.1, 0.1], mass=0.5, eatable=True): name = "Sphere_" + str(len(self.eatable_objects) + 1) self.io.add_sphere(name, position, sizes, mass) if eatable: self.eatable_objects.append(name) self.io._inject_lua_code("simSetObjectSpecialProperty({}, {})".format( self.io.get_object_handle(name), vrep.sim_objectspecialproperty_detectable_all)) self.n_spheres = len(self.eatable_objects) for robot in self.robots: robot.register_object(name) def start_sphere_apparition(self, period=5., min_pos=[-1., -1., .1], max_pos=[1., 1., 1.]): self.attach_routine(sphere_apparition, freq=1. / period, min_pos=min_pos, max_pos=max_pos) self.attach_routine(eating, freq=4.) self.start_routine(sphere_apparition) self.start_routine(eating) def stop_sphere_apparition(self): self.stop_routine(sphere_apparition) self.stop_routine(eating) def add_log(self, topic, data): self.logger.add(topic, data) def get_log(self, topic): return self.logger.get_log(topic) def _vrep_epuck_suffix(self, num): if num == 0: return "" else: return "#" + str(num - 1) def epuck_from_object_name(self, name): if not name.startswith("ePuck"): return None elif "#" in name: suffix = "#" + name.split("#")[1] else: suffix = "" return self.suffix_to_epuck[suffix] def get_epuck(self, use_proximeters=range(8), verbose=False): suffix = self._vrep_epuck_suffix(self.n_robots) epuck = Epuck(pypot_io=VrepIO(self.io.vrep_host, self.io.vrep_port + self.n_robots + 1), simulator=self, robot_id=self.n_robots, use_proximeters=use_proximeters, suffix=suffix) self.suffix_to_epuck[suffix] = epuck self.robots.append(epuck) self.n_robots += 1 if verbose: print self.robots[-1] return self.robots[-1] def get_epuck_list(self, n_epucks, verbose=False): return [self.get_epuck(verbose=verbose) for _ in range(n_epucks)] def remove_object(self, name): self.io.call_remote_api("simxRemoveObject", self.io.get_object_handle(name), sending=True) def attach_routine(self, callback, freq, **kwargs): routine = Routine(self, callback, self._condition, freq, **kwargs) self.routine_manager.attach(routine) def detach_routine(self, callback): self.routine_manager.detach(callback) print "Routine " + callback.__name__ + " detached" def detach_all_routines(self): self.routine_manager.detach_all() def start_routine(self, callback): if self.routine_manager.start(callback): print "Routine " + callback.__name__ + " started" def start_all_routines(self): self.routine_manager.start_all() def stop_routine(self, callback): if self.routine_manager.stop(callback): print "Routine " + callback.__name__ + " stopped" def stop_all_routines(self): self.routine_manager.stop_all() def check_routines(self): self.routine_manager.check()
class AcrobotVrepEnv(gym.Env): metadata = {'render.modes': ['human']} def __init__(self): self._seed = None self.max_speed = 10.0 self.max_torque = 0.5 self.dt = .2 self.step_counter = 0 self.time_counter = time.time() # Coordinates of the outer link tip in local (homogeneous) coordinates self.outer_tip_local = np.array([[0.0], [0.0], [-0.1], [1.0]]) # The tip of outer_link should go above this height (z coordinate in the global frame) self.threshold = 0.75 pypot.vrep.close_all_connections() self.vrepio = VrepIO(vrep_host=host, vrep_port=port, scene=scene, start=False) #self.vrepio.call_remote_api('simxSynchronous',True) self.vrepio.start_simulation() print('(AcrobotVrepEnv) initialized') obs = np.array([np.inf] * 4) self.action_space = spaces.Box(low=-self.max_torque, high=self.max_torque, shape=(1, )) self.observation_space = spaces.Box(-obs, obs) def _self_observe(self): self.active_joint_pos = self.vrepio.call_remote_api( 'simxGetJointPosition', self.vrepio.get_object_handle('active_joint'), streaming=True) # VREP Rempte API does not provide a direct function for retrieving joint velocities # Refer to http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=2393 for details self.active_joint_velocity = self.vrepio.call_remote_api( 'simxGetObjectFloatParameter', self.vrepio.get_object_handle('active_joint'), 2012, streaming=True) self.passive_joint_pos = self.vrepio.call_remote_api( 'simxGetJointPosition', self.vrepio.get_object_handle('passive_joint'), streaming=True) # VREP Rempte API does not provide a direct function for retrieving joint velocities # Refer to http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=2393 for details self.passive_joint_velocity = self.vrepio.call_remote_api( 'simxGetObjectFloatParameter', self.vrepio.get_object_handle('passive_joint'), 2012, streaming=True) self.observation = np.array([ self.active_joint_pos, self.active_joint_velocity, self.passive_joint_pos, self.passive_joint_velocity ]).astype('float32') def _render(self, mode='human', close=False): return def _step(self, actions): self.step_counter += 1 # Advance simulation by 1 step #self.vrepio.call_remote_api('simxSynchronousTrigger') actions = np.clip(actions, -self.max_torque, self.max_torque)[0] apply_torque = actions max_vel = 0.0 # If the torque to be applied is negative, change the sign of the max velocity and then apply a positive torque # with the same magnitude if apply_torque < 0.0: max_vel = -self.max_speed apply_torque = -apply_torque else: max_vel = self.max_speed # step # Set the target velocity self.vrepio.call_remote_api( 'simxSetJointTargetVelocity', self.vrepio.get_object_handle('active_joint'), max_vel, sending=True) # Set the torque of active_joint self.vrepio.call_remote_api( 'simxSetJointForce', self.vrepio.get_object_handle('active_joint'), apply_torque, sending=True) # observe again self._self_observe() # cost # If the tip of the outer link is below threshold then reward is -1 else 0 # First get the position of the outer link frame x, y, z = self.vrepio.call_remote_api( 'simxGetObjectPosition', self.vrepio.get_object_handle('outer_link'), -1, streaming=True) # Then calculate the orientation of the outer link frame in Euler angles a, b, g = self.vrepio.call_remote_api( 'simxGetObjectOrientation', self.vrepio.get_object_handle('outer_link'), -1, streaming=True) # Compute the homogeneous rotation matrix rot_mat = euler_matrix(a, b, g) # Insert the position rot_mat[0][3] = x rot_mat[1][3] = y rot_mat[2][3] = z # The outer tip of outer_link is at coordinate (0,0,-0.1) in the local frame # Convert this into a position in the global frame tip_global = np.dot(rot_mat, self.outer_tip_local) # Consider the height (z-coordinate) of the tip in the global frame tip_height_global = tip_global[2][0] # If this height is above the threshold then reward is 0 else -1 # If height is above threshold, terminate episode terminal = False if tip_height_global >= self.threshold: terminal = True print '=================================== Reached threshold ===================================' reward = 0 else: reward = -1 time.sleep(self.dt) return self.observation, reward, terminal, {} def _reset(self): self.vrepio.stop_simulation() print 'Reset called ######################' print 'Duration of episode: {}'.format(time.time() - self.time_counter) self.time_counter = time.time() print 'Number of steps in episode: {}'.format(self.step_counter) self.step_counter = 0 # Try self.vrepio.call_remote_api('simxStopSimulation',remote_api.simx_opmode_blocking) pypot.vrep.close_all_connections() self.vrepio = VrepIO(vrep_host=host, vrep_port=port, scene=scene, start=False) # self.vrepio.call_remote_api('simxSynchronous',True) self.vrepio.start_simulation() self._self_observe() return self.observation def _destroy(self): self.vrepio.stop_simulation()
class Simulator(Observable): def __init__(self, vrep_host='127.0.0.1', vrep_port=19997, scene=None, start=False): self.io = VrepIO(vrep_host, vrep_port, scene, start) # 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 self.robots = [] self.vrep_mode = vrep_mode self.n_robots = 0 self.suffix_to_epuck = {} self.seconds_to_run = 0 self._condition = threading.Condition() self.routine_manager = RoutineManager() self.eatable_objects = [] self.logger = Logger() Observable.__init__(self) def wait(self, seconds): start = self.io.get_simulation_current_time() while self.io.get_simulation_current_time() - start < seconds: sleep(0.005) def close(self): self.detach_all_routines() sleep(0.3) self.io.stop_simulation() self.io.close() def get_object_position(self, object_name): return array(self.io.get_object_position(object_name)) def set_object_position(self, object_name, position): return self.io.set_object_position(object_name, position) def add_sphere(self, position, sizes=[0.1, 0.1, 0.1], mass=0.5, eatable=True): name = "Sphere_" + str(len(self.eatable_objects) + 1) self.io.add_sphere(name, position, sizes, mass) if eatable: self.eatable_objects.append(name) self.io._inject_lua_code("simSetObjectSpecialProperty({}, {})".format(self.io.get_object_handle(name), vrep.sim_objectspecialproperty_detectable_all)) self.n_spheres = len(self.eatable_objects) for robot in self.robots: robot.register_object(name) def start_sphere_apparition(self, period=5., min_pos=[-1., -1., .1], max_pos=[1., 1., 1.]): self.attach_routine(sphere_apparition, freq=1./period, min_pos=min_pos, max_pos=max_pos) self.attach_routine(eating, freq=4.) self.start_routine(sphere_apparition) self.start_routine(eating) def stop_sphere_apparition(self): self.stop_routine(sphere_apparition) self.stop_routine(eating) def add_log(self, topic, data): self.logger.add(topic, data) def get_log(self, topic): return self.logger.get_log(topic) def _vrep_epuck_suffix(self, num): if num == 0: return "" else: return "#" + str(num - 1) def epuck_from_object_name(self, name): if not name.startswith("ePuck"): return None elif "#" in name: suffix = "#" + name.split("#")[1] else: suffix = "" return self.suffix_to_epuck[suffix] def get_epuck(self, use_proximeters=range(8), verbose=False): suffix = self._vrep_epuck_suffix(self.n_robots) epuck = Epuck(pypot_io=VrepIO(self.io.vrep_host, self.io.vrep_port + self.n_robots + 1), simulator=self, robot_id=self.n_robots, use_proximeters=use_proximeters, suffix=suffix) self.suffix_to_epuck[suffix] = epuck self.robots.append(epuck) self.n_robots += 1 if verbose: print self.robots[-1] return self.robots[-1] def get_epuck_list(self, n_epucks, verbose=False): return [self.get_epuck(verbose=verbose) for _ in range(n_epucks)] def remove_object(self, name): self.io.call_remote_api("simxRemoveObject", self.io.get_object_handle(name), sending=True) def attach_routine(self,callback, freq, **kwargs): routine = Routine(self, callback, self._condition, freq, **kwargs) self.routine_manager.attach(routine) def detach_routine(self, callback): self.routine_manager.detach(callback) print "Routine " + callback.__name__ + " detached" def detach_all_routines(self): self.routine_manager.detach_all() def start_routine(self, callback): if self.routine_manager.start(callback): print "Routine " + callback.__name__ + " started" def start_all_routines(self): self.routine_manager.start_all() def stop_routine(self, callback): if self.routine_manager.stop(callback): print "Routine " + callback.__name__ + " stopped" def stop_all_routines(self): self.routine_manager.stop_all() def check_routines(self): self.routine_manager.check()