notebook.community

Edit and run

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

Test below done with 12 xl-320
Pixl
Raspi Jessie
Pypot 2.11

Low level test





In [1]:



    


import pypot.dynamixel
import time



    











In [2]:



    


print(pypot.dynamixel.get_available_ports())



    


















    






['/dev/ttyAMA0']

















In [19]:



    


dxl_io = pypot.dynamixel.Dxl320IO('/dev/ttyAMA0', use_sync_read=False)



    











In [7]:



    


motors = (dxl_io.scan(range(60)))
print (motors)
print(len(motors))



    


















    






[11, 12, 13, 21, 22, 23, 31, 32, 33, 41, 42, 43]
12

















In [8]:



    


%timeit dxl_io.get_present_position(motors)



    


















    






10 loops, best of 3: 19.4 ms per loop

















In [20]:



    


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 utilisé par une instruction  get_present_position est donc de : %f ms' % p)



    


















    






nombre d'instructions en 1 seconde : 34 -> 34 Hz 
Temps réellement utilisé pour l'instruction get_present_position : 0.66 s
le temps utilisé par une instruction  get_present_position est donc de : 19.411765 ms

















In [21]:



    


dxl_io.close()



    











In [22]:



    


dxl_io = pypot.dynamixel.Dxl320IO('/dev/ttyAMA0', use_sync_read=True)



    











In [23]:



    


%timeit dxl_io.get_present_position(motors)



    


















    






---------------------------------------------------------------------------
ValueError                                Traceback (most recent call last)
/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py in __real_read(self, instruction_packet, _force_lock)
    509             try:
--> 510                 header = self._protocol.DxlPacketHeader.from_string(data)
    511                 data += self._serial.read(header.packet_length)

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/protocol/v2.py in from_string(cls, data)
     39         if len(header) != cls.length or header[:len(cls.marker)] != cls.marker:
---> 40             raise ValueError('try to parse corrupted data ({})'.format(header))
     41 

ValueError: try to parse corrupted data (bytearray(b'U\x00\t\x02)r\xff'))

During handling of the above exception, another exception occurred:

DxlCommunicationError                     Traceback (most recent call last)
<ipython-input-23-3568df29ecc8> in <module>()
----> 1 get_ipython().magic('timeit dxl_io.get_present_position(motors)')

/usr/local/lib/python3.4/dist-packages/IPython/core/interactiveshell.py in magic(self, arg_s)
   2161         magic_name, _, magic_arg_s = arg_s.partition(' ')
   2162         magic_name = magic_name.lstrip(prefilter.ESC_MAGIC)
-> 2163         return self.run_line_magic(magic_name, magic_arg_s)
   2164 
   2165     #-------------------------------------------------------------------------

/usr/local/lib/python3.4/dist-packages/IPython/core/interactiveshell.py in run_line_magic(self, magic_name, line)
   2082                 kwargs['local_ns'] = sys._getframe(stack_depth).f_locals
   2083             with self.builtin_trap:
-> 2084                 result = fn(*args,**kwargs)
   2085             return result
   2086 

<decorator-gen-59> in timeit(self, line, cell)

/usr/local/lib/python3.4/dist-packages/IPython/core/magic.py 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):

/usr/local/lib/python3.4/dist-packages/IPython/core/magics/execution.py in timeit(self, line, cell)
   1039             number = 1
   1040             for _ in range(1, 10):
-> 1041                 time_number = timer.timeit(number)
   1042                 worst_tuning = max(worst_tuning, time_number / number)
   1043                 if time_number >= 0.2:

/usr/local/lib/python3.4/dist-packages/IPython/core/magics/execution.py in timeit(self, number)
    135         gc.disable()
    136         try:
--> 137             timing = self.inner(it, self.timer)
    138         finally:
    139             if gcold:

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

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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(' ', '_'))

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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 ()

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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:

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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,

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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 bytearray(b'U\x00\t\x02)r\xff') after sending DxlSyncReadDataPacket(ids=[11, 12, 13, 21, 22, 23, 31, 32, 33, 41, 42, 43], address=37, length=2)
















In [25]:



    


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 utilisé par une instruction  get_present_position est donc de : %f ms' % p)



    


















    






nombre d'instructions en 1 seconde : 55 -> 55 Hz 
Temps réellement utilisé pour l'instruction get_present_position : 0.45 s
le temps utilisé par une instruction  get_present_position est donc de : 8.181818 ms

















In [26]:



    


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 utilisé par une instruction  get_present_position est donc de : %f ms' % p)



    


















    






nombre d'instructions en 1 seconde : 76 -> 76 Hz 
Temps réellement utilisé pour l'instruction get_present_position : 0.62 s
le temps utilisé par une instruction  get_present_position est donc de : 8.157895 ms

















In [28]:



    


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 utilisé par une instruction  get_present_position est donc de : %f ms' % p)



    


















    






---------------------------------------------------------------------------
ValueError                                Traceback (most recent call last)
/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py in __real_read(self, instruction_packet, _force_lock)
    509             try:
--> 510                 header = self._protocol.DxlPacketHeader.from_string(data)
    511                 data += self._serial.read(header.packet_length)

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/protocol/v2.py in from_string(cls, data)
     39         if len(header) != cls.length or header[:len(cls.marker)] != cls.marker:
---> 40             raise ValueError('try to parse corrupted data ({})'.format(header))
     41 

ValueError: try to parse corrupted data (bytearray(b'\xff\xff\xfd/\xf9\xdf\xdf'))

During handling of the above exception, another exception occurred:

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

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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(' ', '_'))

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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 ()

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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:

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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,

/usr/local/lib/python3.4/dist-packages/pypot/dynamixel/io/abstract_io.py 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 bytearray(b'\xff\xff\xfd/\xf9\xdf\xdf') after sending DxlSyncReadDataPacket(ids=[11, 12, 13, 21, 22, 23, 31, 32, 33, 41, 42, 43], address=37, length=2)
















In [29]:



    


dxl_io.close()

Robot level test (syncloop)





In [30]:



    


from roboticia_horse import RoboticiaHorse



    











In [31]:



    


robot=RoboticiaHorse()



    











In [32]:



    


robot.stop_sync()

The config :





In [33]:



    


robot.config



    


















    
Out[33]:








{'controllers': {'my_dxl_controller': {'attached_motors': ['leg1',
    'leg2',
    'leg3',
    'leg4'],
   'port': 'auto',
   'protocol': 2,
   'sync_read': True}},
 'motorgroups': {'leg1': ['m11', 'm12', 'm13'],
  'leg2': ['m21', 'm22', 'm23'],
  'leg3': ['m31', 'm32', 'm33'],
  'leg4': ['m41', 'm42', 'm43']},
 'motors': {'m11': {'angle_limit': [-150.0, 150.0],
   'id': 11,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm12': {'angle_limit': [-150.0, 150.0],
   'id': 12,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm13': {'angle_limit': [-150.0, 150.0],
   'id': 13,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm21': {'angle_limit': [-150.0, 150.0],
   'id': 21,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm22': {'angle_limit': [-150.0, 150.0],
   'id': 22,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm23': {'angle_limit': [-150.0, 150.0],
   'id': 23,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm31': {'angle_limit': [-150.0, 150.0],
   'id': 31,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm32': {'angle_limit': [-150.0, 150.0],
   'id': 32,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm33': {'angle_limit': [-150.0, 150.0],
   'id': 33,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm41': {'angle_limit': [-150.0, 150.0],
   'id': 41,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm42': {'angle_limit': [-150.0, 150.0],
   'id': 42,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'},
  'm43': {'angle_limit': [-150.0, 150.0],
   'id': 43,
   'offset': 0.0,
   'orientation': 'direct',
   'type': 'XL-320'}}}

The controllers





In [40]:



    


robot._controllers



    


















    
Out[40]:








[<pypot.dynamixel.syncloop.BaseDxlController at 0x6e2e0ed0>]

Set the return_delay_time for motors





In [41]:



    


dxl_io=robot._controllers[0].io



    











In [43]:



    


dxl_io.get_return_delay_time((11,12,13,21,22,23,31,32,33,41,42,43))



    


















    
Out[43]:








(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)

















In [44]:



    


dxl_io.set_return_delay_time({11:250,12:250,13:250,21:250,22:250,23:250,31:250,32:250,33:250,41:250,42:250,43:250})

Testing of the syncloop





In [46]:



    


robot.start_sync()



    











In [47]:



    


robot.stop_sync()

Set the sync_read





In [48]:



    


dxl_io._sync_read = True

Testing of the syncloop





In [50]:



    


robot.start_sync()

closing after 30 secondes.





In [51]:



    


robot.stop_sync()

Set the return_delay_time for motors





In [52]:



    


dxl_io.set_return_delay_time({11:0,12:0,13:0,21:0,22:0,23:0,31:0,32:0,33:0,41:0,42:0,43:0})

Testing of the syncloop





In [54]:



    


robot.start_sync()



    











In [55]:



    


robot.stop_sync()



    











In [56]:



    


robot.close()



    











In [ ]:

Content source: jjehl/poppy_education

Similar notebooks:

notebook.community | gallery | about