How to test communication with motors of your robots - Speed tests

Test below done with 4 xl-320
USB2AX
Windows 10
Pypot 2.11

Low level test


In [1]:
import pypot.dynamixel
import time

In [2]:
print(pypot.dynamixel.get_available_ports())


['COM5']

In [3]:
dxl_io = pypot.dynamixel.Dxl320IO('COM5', use_sync_read=False)

In [4]:
motors = (dxl_io.scan(range(60)))
print motors
print len(motors)


[1, 2, 3, 4]
4

In [5]:
%timeit dxl_io.get_present_position(motors)


100 loops, best of 3: 4.94 ms per loop

In [16]:
t0 = time.time()
i=0
latency = 0.01

while time.time() - t0 < 1 :
    dxl_io.get_present_position(motors)
    i+=1
    time.sleep(latency)


print "nombre d'instructions en 1 seconde : %d -> %d Hz" % (i,i) 
t = ((1.0-(i*latency))/i)*1000
print 'le temps pour éxécuter une instruction est de : %f ms' % t


nombre d'instructions en 1 seconde : 62 -> 62 Hz
le temps pour éxécuter une instruction est de : 6.129032 ms

In [17]:
dxl_io.close()

In [25]:
dxl_io = pypot.dynamixel.Dxl320IO('COM5', use_sync_read=True)

In [26]:
%timeit dxl_io.get_present_position(motors)


---------------------------------------------------------------------------
DxlCommunicationError                     Traceback (most recent call last)
<ipython-input-26-4844afeb3871> in <module>()
----> 1 get_ipython().magic(u'timeit dxl_io.get_present_position(motors)')

c:\python27\lib\site-packages\ipython-3.0.0_b1-py2.7.egg\IPython\core\interactiveshell.pyc in magic(self, arg_s)
   2302         magic_name, _, magic_arg_s = arg_s.partition(' ')
   2303         magic_name = magic_name.lstrip(prefilter.ESC_MAGIC)
-> 2304         return self.run_line_magic(magic_name, magic_arg_s)
   2305 
   2306     #-------------------------------------------------------------------------

c:\python27\lib\site-packages\ipython-3.0.0_b1-py2.7.egg\IPython\core\interactiveshell.pyc in run_line_magic(self, magic_name, line)
   2223                 kwargs['local_ns'] = sys._getframe(stack_depth).f_locals
   2224             with self.builtin_trap:
-> 2225                 result = fn(*args,**kwargs)
   2226             return result
   2227 

c:\python27\lib\site-packages\ipython-3.0.0_b1-py2.7.egg\IPython\core\magics\execution.pyc in timeit(self, line, cell)

c:\python27\lib\site-packages\ipython-3.0.0_b1-py2.7.egg\IPython\core\magic.pyc in <lambda>(f, *a, **k)
    191     # but it's overkill for just that one bit of state.
    192     def magic_deco(arg):
--> 193         call = lambda f, *a, **k: f(*a, **k)
    194 
    195         if callable(arg):

c:\python27\lib\site-packages\ipython-3.0.0_b1-py2.7.egg\IPython\core\magics\execution.pyc in timeit(self, line, cell)
   1039                     break
   1040                 number *= 10
-> 1041         all_runs = timer.repeat(repeat, number)
   1042         best = min(all_runs) / number
   1043         if not quiet :

c:\python27\lib\timeit.pyc in repeat(self, repeat, number)
    221         r = []
    222         for i in range(repeat):
--> 223             t = self.timeit(number)
    224             r.append(t)
    225         return r

c:\python27\lib\site-packages\ipython-3.0.0_b1-py2.7.egg\IPython\core\magics\execution.pyc in timeit(self, number)
    130         gc.disable()
    131         try:
--> 132             timing = self.inner(it, self.timer)
    133         finally:
    134             if gcold:

<magic-timeit> in inner(_it, _timer)

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in my_getter(self, ids, **kwargs)
    355         if control.access in (_DxlAccess.readonly, _DxlAccess.readwrite):
    356             def my_getter(self, ids, **kwargs):
--> 357                 return self._get_control_value(control, ids, **kwargs)
    358 
    359             func_name = control.getter_name if control.getter_name else 'get_{}'.format(control.name.replace(' ', '_'))

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in _get_control_value(self, control, ids, **kwargs)
    387                 sp = self._send_packet(rp,
    388                                        error_handler=error_handler,
--> 389                                        _force_lock=True)
    390                 if not sp:
    391                     return ()

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in _send_packet(self, instruction_packet, wait_for_status_packet, error_handler, _force_lock)
    524 
    525         if not error_handler:
--> 526             return self.__real_send(instruction_packet, wait_for_status_packet, _force_lock)
    527 
    528         try:

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in __real_send(self, instruction_packet, wait_for_status_packet, _force_lock)
    492                 return
    493 
--> 494             status_packet = self.__real_read(instruction_packet, _force_lock=True)
    495 
    496             logger.debug('Receiving %s', status_packet,

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in __real_read(self, instruction_packet, _force_lock)
    514             except ValueError:
    515                 msg = 'could not parse received data {}'.format(bytearray(data))
--> 516                 raise DxlCommunicationError(self, msg, instruction_packet)
    517 
    518             return status_packet

DxlCommunicationError: could not parse received data U”š after sending DxlSyncReadDataPacket(ids=[1, 2, 3, 4], address=37, length=2)

In [38]:
t0 = time.time()
i=0
latency = 0.01
while time.time() - t0 < 1 :
    dxl_io.get_present_position(motors)
    i+=1
    time.sleep(latency)

print "nombre d'instructions en 1 seconde : %d -> %d Hz " % (i,i) 
t = (1.0-(i*latency))
p = t/i*1000
print "Temps réellement utilisé pour l'instruction get_present_position : %.2f s" %(t)
print 'le temps pour éxécuter une instruction est donc de : %f ms' % p


nombre d'instructions en 1 seconde : 65 -> 65 Hz 
Temps réellement utilisé pour l'instruction get_present_position : 0.35 s
le temps pour éxécuter une instruction est donc de : 5.384615 ms

In [39]:
t0 = time.time()
i=0
latency = 0.005
while time.time() - t0 < 1 :
    dxl_io.get_present_position(motors)
    i+=1
    time.sleep(latency)

print "nombre d'instructions en 1 seconde : %d -> %d Hz " % (i,i) 
t = (1.0-(i*latency))
p = t/i*1000
print "Temps réellement utilisé pour l'instruction get_present_position : %.2f s" %(t)
print 'le temps pour éxécuter une instruction est donc de : %f ms' % p


nombre d'instructions en 1 seconde : 72 -> 72 Hz 
Temps réellement utilisé pour l'instruction get_present_position : 0.64 s
le temps pour éxécuter une instruction est donc de : 8.888889 ms

In [40]:
t0 = time.time()
i=0
latency = 0.001
while time.time() - t0 < 1 :
    dxl_io.get_present_position(motors)
    i+=1
    time.sleep(latency)

print "nombre d'instructions en 1 seconde : %d -> %d Hz " % (i,i) 
t = (1.0-(i*latency))
p = t/i*1000
print "Temps réellement utilisé pour l'instruction get_present_position : %.2f s" %(t)
print 'le temps pour éxécuter une instruction est donc de : %f ms' % p


nombre d'instructions en 1 seconde : 94 -> 94 Hz 
Temps réellement utilisé pour l'instruction get_present_position : 0.91 s
le temps pour éxécuter une instruction est donc de : 9.638298 ms

In [41]:
t0 = time.time()
i=0
latency = 0.0005
while time.time() - t0 < 1 :
    dxl_io.get_present_position(motors)
    i+=1
    time.sleep(latency)

print "nombre d'instructions en 1 seconde : %d -> %d Hz " % (i,i) 
t = (1.0-(i*latency))
p = t/i*1000
print "Temps réellement utilisé pour l'instruction get_present_position : %.2f s" %(t)
print 'le temps pour éxécuter une instruction est donc de : %f ms' % p


---------------------------------------------------------------------------
DxlCommunicationError                     Traceback (most recent call last)
<ipython-input-41-a36570cdedf6> in <module>()
      3 latency = 0.0005
      4 while time.time() - t0 < 1 :
----> 5     dxl_io.get_present_position(motors)
      6     i+=1
      7     time.sleep(latency)

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in my_getter(self, ids, **kwargs)
    355         if control.access in (_DxlAccess.readonly, _DxlAccess.readwrite):
    356             def my_getter(self, ids, **kwargs):
--> 357                 return self._get_control_value(control, ids, **kwargs)
    358 
    359             func_name = control.getter_name if control.getter_name else 'get_{}'.format(control.name.replace(' ', '_'))

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in _get_control_value(self, control, ids, **kwargs)
    387                 sp = self._send_packet(rp,
    388                                        error_handler=error_handler,
--> 389                                        _force_lock=True)
    390                 if not sp:
    391                     return ()

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in _send_packet(self, instruction_packet, wait_for_status_packet, error_handler, _force_lock)
    524 
    525         if not error_handler:
--> 526             return self.__real_send(instruction_packet, wait_for_status_packet, _force_lock)
    527 
    528         try:

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in __real_send(self, instruction_packet, wait_for_status_packet, _force_lock)
    492                 return
    493 
--> 494             status_packet = self.__real_read(instruction_packet, _force_lock=True)
    495 
    496             logger.debug('Receiving %s', status_packet,

c:\python27\lib\site-packages\pypot-2.11.2-py2.7.egg\pypot\dynamixel\io\abstract_io.pyc in __real_read(self, instruction_packet, _force_lock)
    514             except ValueError:
    515                 msg = 'could not parse received data {}'.format(bytearray(data))
--> 516                 raise DxlCommunicationError(self, msg, instruction_packet)
    517 
    518             return status_packet

DxlCommunicationError: could not parse received data U”š¦ after sending DxlSyncReadDataPacket(ids=[1, 2, 3, 4], address=37, length=2)

In [42]:
dxl_io.close()

Robot level test (syncloop)


In [3]:
from poppy.creatures import Poppy4dofArmMini

In [22]:
mini_dof=Poppy4dofArmMini()


<class 'pypot.sensor.test_sensor.watch.Watch'>

In [23]:
mini_dof.stop_sync()

The config :


In [24]:
mini_dof.config


Out[24]:
{u'controllers': {u'my_dxl_controller': {u'attached_motors': [u'bottom',
    u'top'],
   u'port': u'auto',
   u'protocol': 2,
   u'sync_read': False}},
 u'motorgroups': {u'bottom': [u'm1', u'm2'], u'top': [u'm3', u'm4']},
 u'motors': {u'm1': {u'angle_limit': [-150.0, 150.0],
   u'id': 1,
   u'offset': 0.0,
   u'orientation': u'indirect',
   u'type': u'XL-320'},
  u'm2': {u'angle_limit': [-90.0, 90.0],
   u'id': 2,
   u'offset': 0.0,
   u'orientation': u'direct',
   u'type': u'XL-320'},
  u'm3': {u'angle_limit': [-150.0, 150.0],
   u'id': 3,
   u'offset': 0.0,
   u'orientation': u'indirect',
   u'type': u'XL-320'},
  u'm4': {u'angle_limit': [-150.0, 150.0],
   u'id': 4,
   u'offset': 0.0,
   u'orientation': u'direct',
   u'type': u'XL-320'}},
 u'sensors': {u'test_sensor': {u'type': u'Watch'}}}

The controllers


In [25]:
mini_dof._controllers


Out[25]:
[<pypot.dynamixel.syncloop.BaseDxlController at 0x5551970>]

Set the return_delay_time for motors


In [26]:
dxl_io=mini_dof._controllers[0].io

In [27]:
dxl_io.get_return_delay_time((1,2,3,4))


Out[27]:
(0, 0, 0, 0)

In [28]:
dxl_io.set_return_delay_time({1:250,2:250,3:250,4:250})

Testing of the syncloop


In [29]:
mini_dof.start_sync()

In [30]:
mini_dof.stop_sync()

Set the sync_read


In [31]:
dxl_io._sync_read = True

Testing of the syncloop


In [32]:
mini_dof.start_sync()


WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]

closing after 30 secondes.


In [33]:
mini_dof.stop_sync()


WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]

Set the return_delay_time for motors


In [34]:
dxl_io.set_return_delay_time({1:0,2:0,3:0,4:0})

Testing of the syncloop


In [35]:
mini_dof.start_sync()


WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]
WARNING:pypot.dynamixel.controller:Timeout when getting pos/speed/load from [1, 2, 3, 4]

In [36]:
mini_dof.stop_sync()

In [37]:
mini_dof.close()

In [ ]: