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()
