Merge pull request #1 from pierre-rouanet/multi-vrep-robot

Add support for multiple robots in a V-REP scene

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

pypot/_version.py

@@ -1 +1 @@
-__version__ = '3.0.4'
+__version__ = '3.1.0'

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=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)
+                    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!')
 

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=[],
+              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)
@@ -60,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.
 
@@ -80,7 +84,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
@@ -93,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 = []

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