From 4aef47098085eddb712a2d26dabc5d91f099c50f Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Tue, 24 Apr 2018 16:04:29 +0200 Subject: [PATCH 1/3] Allows to have multiple robot connected to the same vrep scene. --- pypot/creatures/abstractcreature.py | 4 ++-- pypot/vrep/__init__.py | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/pypot/creatures/abstractcreature.py b/pypot/creatures/abstractcreature.py index dba6e48d..9b8c4818 100644 --- a/pypot/creatures/abstractcreature.py +++ b/pypot/creatures/abstractcreature.py @@ -29,7 +29,7 @@ class AbstractPoppyCreature(Robot): """ Abstract Class for Any Poppy Creature. """ def __new__(cls, base_path=None, config=None, - simulator=None, scene=None, host='localhost', port=19997, id=0, + simulator=None, scene=None, host='localhost', port=19997, id=0, shared_vrep_io=None, use_snap=False, snap_host='0.0.0.0', snap_port=6969, snap_quiet=True, use_http=False, http_host='0.0.0.0', http_port=8080, http_quiet=True, use_remote=False, remote_host='0.0.0.0', remote_port=4242, @@ -97,7 +97,7 @@ def __new__(cls, host = '127.0.0.1' try: - poppy_creature = from_vrep(config, host, port, scene if scene != "keep-existing" else None) + poppy_creature = from_vrep(config, host, port, scene if scene != "keep-existing" else None, shared_vrep_io=shared_vrep_io) except VrepConnectionError: raise IOError('Connection to V-REP failed!') diff --git a/pypot/vrep/__init__.py b/pypot/vrep/__init__.py index ab51679f..4ec34829 100644 --- a/pypot/vrep/__init__.py +++ b/pypot/vrep/__init__.py @@ -50,7 +50,8 @@ def sleep(self, t): def from_vrep(config, vrep_host='127.0.0.1', vrep_port=19997, scene=None, - tracked_objects=[], tracked_collisions=[]): + tracked_objects=[], tracked_collisions=[], + shared_vrep_io=None): """ Create a robot from a V-REP instance. :param config: robot configuration (either the path to the json or directly the dictionary) @@ -80,7 +81,10 @@ def from_vrep(config, vrep_host='127.0.0.1', vrep_port=19997, scene=None, simulated_robot = from_vrep(config, '127.0.0.1', 19997, 'poppy.ttt') """ - vrep_io = VrepIO(vrep_host, vrep_port) + if shared_vrep_io is None: + vrep_io = VrepIO(vrep_host, vrep_port) + else: + vrep_io = shared_vrep_io vreptime = vrep_time(vrep_io) pypot_time.time = vreptime.get_time From 4c6f2730820c2e8cb64ef43176e8b3c4332fc8c6 Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Wed, 25 Apr 2018 14:05:02 +0200 Subject: [PATCH 2/3] Add support for robot id in vrep scene. Permits the support of multiple robots within a same vrep scene. --- pypot/creatures/abstractcreature.py | 7 ++++-- pypot/vrep/__init__.py | 7 ++++-- pypot/vrep/controller.py | 33 ++++++++++++++++++----------- 3 files changed, 31 insertions(+), 16 deletions(-) diff --git a/pypot/creatures/abstractcreature.py b/pypot/creatures/abstractcreature.py index 9b8c4818..c3b8581d 100644 --- a/pypot/creatures/abstractcreature.py +++ b/pypot/creatures/abstractcreature.py @@ -29,7 +29,7 @@ class AbstractPoppyCreature(Robot): """ Abstract Class for Any Poppy Creature. """ def __new__(cls, base_path=None, config=None, - simulator=None, scene=None, host='localhost', port=19997, id=0, shared_vrep_io=None, + simulator=None, scene=None, host='localhost', port=19997, id=None, shared_vrep_io=None, use_snap=False, snap_host='0.0.0.0', snap_port=6969, snap_quiet=True, use_http=False, http_host='0.0.0.0', http_port=8080, http_quiet=True, use_remote=False, remote_host='0.0.0.0', remote_port=4242, @@ -46,6 +46,9 @@ def __new__(cls, :param str scene: specify a particular simulation scene (if None uses the default scene of the poppy creature, use "keep-existing" to keep the current VRep scene - e.g. poppy_humanoid.ttt) :param str host: host of the simulator :param int port: port of the simulator + :param int id: robot id in simulator (useful when using a scene with multiple robots) + :param vrep_io: use an already connected VrepIO (useful when using a scene with multiple robots) + :type vrep_io: :class:`~pypot.vrep.io.VrepIO` :param bool use_snap: start or not the Snap! API :param str snap_host: host of Snap! API :param int snap_port: port of the Snap! @@ -97,7 +100,7 @@ def __new__(cls, host = '127.0.0.1' try: - poppy_creature = from_vrep(config, host, port, scene if scene != "keep-existing" else None, shared_vrep_io=shared_vrep_io) + poppy_creature = from_vrep(config, host, port, scene if scene != "keep-existing" else None, id=id, shared_vrep_io=shared_vrep_io) except VrepConnectionError: raise IOError('Connection to V-REP failed!') diff --git a/pypot/vrep/__init__.py b/pypot/vrep/__init__.py index 4ec34829..d3c29e30 100644 --- a/pypot/vrep/__init__.py +++ b/pypot/vrep/__init__.py @@ -51,7 +51,7 @@ def sleep(self, t): def from_vrep(config, vrep_host='127.0.0.1', vrep_port=19997, scene=None, tracked_objects=[], tracked_collisions=[], - shared_vrep_io=None): + id=None, shared_vrep_io=None): """ Create a robot from a V-REP instance. :param config: robot configuration (either the path to the json or directly the dictionary) @@ -61,6 +61,9 @@ def from_vrep(config, vrep_host='127.0.0.1', vrep_port=19997, scene=None, :param str scene: path to the V-REP scene to load and start :param list tracked_objects: list of V-REP dummy object to track :param list tracked_collisions: list of V-REP collision to track + :param int id: robot id in simulator (useful when using a scene with multiple robots) + :param vrep_io: use an already connected VrepIO (useful when using a scene with multiple robots) + :type vrep_io: :class:`~pypot.vrep.io.VrepIO` This function tries to connect to a V-REP instance and expects to find motors with names corresponding as the ones found in the config. @@ -97,7 +100,7 @@ def from_vrep(config, vrep_host='127.0.0.1', vrep_port=19997, scene=None, motors = [motor_from_confignode(config, name) for name in config['motors'].keys()] - vc = VrepController(vrep_io, scene, motors) + vc = VrepController(vrep_io, scene, motors, id=id) vc._init_vrep_streaming() sensor_controllers = [] diff --git a/pypot/vrep/controller.py b/pypot/vrep/controller.py index 6f1059ba..99d48f89 100644 --- a/pypot/vrep/controller.py +++ b/pypot/vrep/controller.py @@ -11,17 +11,20 @@ class VrepController(MotorsController): """ V-REP motors controller. """ - def __init__(self, vrep_io, scene, motors, sync_freq=50.): + def __init__(self, vrep_io, scene, motors, sync_freq=50., id=None): """ :param vrep_io: vrep io instance :type vrep_io: :class:`~pypot.vrep.io.VrepIO` :param str scene: path to the V-REP scene file to start :param list motors: list of motors attached to the controller :param float sync_freq: synchronization frequency + :param int id: robot id in simulator (useful when using a scene with multiple robots) """ MotorsController.__init__(self, vrep_io, motors, sync_freq) + self.id = id + if scene is not None: vrep_io.load_scene(scene, start=True) @@ -53,30 +56,30 @@ def update(self): # Read values from V-REP and set them to the Motor p = round( - rad2deg(self.io.get_motor_position(motor_name=m.name)), 1) + rad2deg(self.io.get_motor_position(motor_name=self._motor_name(m))), 1) m.__dict__['present_position'] = p - l = 100. * self.io.get_motor_force(motor_name=m.name) / tmax + l = 100. * self.io.get_motor_force(motor_name=self._motor_name(m)) / tmax m.__dict__['present_load'] = l m.__dict__['_load_fifo'].append(abs(l)) m.__dict__['present_temperature'] = 25 + \ round(2.5 * sum(m.__dict__['_load_fifo']) / len(m.__dict__['_load_fifo']), 1) - ll, lr = limits4handle[self.io._object_handles[m.name]] + ll, lr = limits4handle[self.io._object_handles[self._motor_name(m)]] m.__dict__['lower_limit'] = rad2deg(ll) m.__dict__['upper_limit'] = rad2deg(ll) + rad2deg(lr) # Send new values from Motor to V-REP p = deg2rad(round(m.__dict__['goal_position'], 1)) - self.io.set_motor_position(motor_name=m.name, position=p) + self.io.set_motor_position(motor_name=self._motor_name(m), position=p) t = m.__dict__['torque_limit'] * tmax / 100. if m.__dict__['compliant']: t = 0. - self.io.set_motor_force(motor_name=m.name, force=t) + self.io.set_motor_force(motor_name=self._motor_name(m), force=t) def _init_vrep_streaming(self): # While the code below may look redundant and that @@ -89,24 +92,24 @@ def _init_vrep_streaming(self): for m in self.motors: for vrep_call in ['simxGetJointPosition', 'simxGetJointForce']: self.io.call_remote_api(vrep_call, - self.io.get_object_handle(m.name), + self.io.get_object_handle(self._motor_name(m)), streaming=True, _force=True) # Now actually retrieves all values - pos = [self.io.get_motor_position(m.name) for m in self.motors] + pos = [self.io.get_motor_position(self._motor_name(m)) for m in self.motors] # Prepare streaming for setting position for each motor for m, p in zip(self.motors, pos): self.io.call_remote_api('simxSetJointTargetPosition', - self.io.get_object_handle(m.name), + self.io.get_object_handle(self._motor_name(m)), p, sending=True, _force=True) for m in self.motors: self.io.call_remote_api('simxSetJointForce', - self.io.get_object_handle(m.name), + self.io.get_object_handle(self._motor_name(m)), torque_max[m.model], sending=True, _force=True) @@ -120,14 +123,20 @@ def _init_vrep_streaming(self): # And actually affect them for m, p in zip(self.motors, pos): - self.io.set_motor_position(m.name, p) + self.io.set_motor_position(self._motor_name(m), p) m.__dict__['goal_position'] = rad2deg(p) for m in self.motors: - self.io.set_motor_force(m.name, torque_max[m.model]) + self.io.set_motor_force(self._motor_name(m), torque_max[m.model]) m.__dict__['torque_limit'] = 100. m.__dict__['compliant'] = False + def _motor_name(self, m): + if self.id is None: + return m.name + else: + return '{}{}'.format(m.name, self.id) + class VrepObjectTracker(SensorsController): From 973b88879698f0785b9beb5fda128e9bfc36ef99 Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Wed, 25 Apr 2018 15:33:59 +0200 Subject: [PATCH 3/3] Prepare for release 3.1 --- changelog.md | 4 ++++ pypot/_version.py | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/changelog.md b/changelog.md index 68ffa4f2..96ac746c 100644 --- a/changelog.md +++ b/changelog.md @@ -1,5 +1,9 @@ # Changelog +## V 3.1 + +* Add support for connecting multiple robots to a same VREP scene. + ## V 2.11 ## Features diff --git a/pypot/_version.py b/pypot/_version.py index 9a8a1b10..7f5601d9 100644 --- a/pypot/_version.py +++ b/pypot/_version.py @@ -1 +1 @@ -__version__ = '3.0.4' +__version__ = '3.1.0'