#!/usr/bin/env python
# coding: utf-8

# # Benchmark your Poppy robot

# The goal of this notebook is to help you identify the performance of your robot and where the bottle necks are. We will measure:
# * the time to read/write the position to one motor (for each of your dynamixel bus)
# * the time to read/write the positions for all motors (for each of your dynamixel bus)
# * the regularity of the synchronization loop of pos/speed/load when
#     * only this loop is runnnig
#     * all other synchronization loops are running
#     * everything else is running

# In[1]:


from ipywidgets import interact


# In[2]:


get_ipython().run_line_magic('pylab', 'inline')


# All bench info will be stored in this dictionary so it's easy to compare with other platforms.

# In[3]:


results = {}


# ## What's the platform

# In[4]:


import platform

p = platform.platform()
print(p)

results['platform'] = p


# In[5]:


import sys

v = sys.version
print(v)

results['python'] = v


# In[ ]:





# In[6]:


import pypot

results['pypot'] = pypot.__version__
print('Pypot version: {}'.format(results['pypot']))


# In[7]:


from pypot.creatures import installed_poppy_creatures

RobotCls = None

def robot_selector(robot):
    global RobotCls
    RobotCls = robot
    
interact(robot_selector, robot=installed_poppy_creatures);


# In[8]:


RobotCls


# In[9]:


robot = RobotCls()
results['robot'] = RobotCls


# Make sure all motors are turned off to avoid breaking anything:

# In[10]:


for m in robot.motors:
    m.compliant = True


# We find the synchronization loop for pos/speed/load and monkey patch them for monitoring.

# In[13]:


import time

from pypot.dynamixel.syncloop import MetaDxlController
from pypot.dynamixel.controller import PosSpeedLoadDxlController

meta_controllers = [c for c in robot._controllers if isinstance(c, MetaDxlController)]

#controllers = [cc for cc in c.controllers for c in meta_controllers if isinstance(cc, PosSpeedLoadDxlController)]
controllers = []
for c in meta_controllers:
    controllers.extend([cc for cc in c.controllers if isinstance(cc, PosSpeedLoadDxlController)])

for c in controllers:
    c.stop()

for c in controllers:    
    def wrapped_update():
        if not hasattr(c, 't'):
            c.t = []
            
        c.t.append(time.time())
        c.update()
        
    c._update = wrapped_update
    

for c in controllers:
    c.start()


# Now, we define our monitor and plotting functions.

# In[20]:


import psutil

def monitor(controllers, duration):
    for c in controllers:
        c.stop()
        c.t = []
        c.start()
        
    cpu = []
    start = time.time()
    while time.time() - start < duration:
        time.sleep(1.0)
    cpu.append(psutil.cpu_percent())
        
    print('Avg CPU usage: {}%'.format(mean(cpu)))
    return {c: array(c.t) for c in controllers}

def freq_plot(logs):
    for c, t in logs.items():
        dt = diff(t)
        freq = 1.0 / dt
        
        print('Avg frq for controller {}: {:.3f} ms STD={:.3} ms'.format(c.ids, freq.mean(), freq.std()))
        hist(freq)
        xlim(0, 100)


# We also define this follow trajectory function, which applies a sinus on one motor (choosen below) and plot how close is its real position from the target one:

# In[16]:


def follow_trajectory(motor, duration=5, freq=50):
    t = linspace(0, duration, duration * freq)

    a1, f1 = 10.0, 1.0
    a2, f2 = 5.0, 0.5

    traj = a1 * sin(2 * pi * f1 * t) + a2 * sin(2 * pi * f2 * t)
    rec = []


    motor.compliant = False
    motor.moving_speed = 0
    motor.goal_position = 0
    time.sleep(1.)

    for p in traj:
        motor.goal_position = p
        rec.append(motor.present_position)

        time.sleep(1.0 / freq)    
        
    motor.compliant = True

    plot(traj)
    plot(rec)    


# Now choose which motor you want to use for the follow trajectory test. It should be able to move freely from -20 to +20 degrees.

# In[17]:


motor = None

def motor_selector(m):
    global motor
    motor = getattr(robot, m)
    
interact(motor_selector, m=[m.name for m in robot.motors]);


# ## Benchmark

# Our benchmark duration in seconds:

# In[18]:


duration = 30


# ### Normal usage

# In[21]:


d = monitor(controllers, duration)
freq_plot(d)

results['normal'] = d


# In[24]:


follow_trajectory(motor)


# ### Without primitives

# In[25]:


for p in robot.primitives:
    p.stop()
    
robot._primitive_manager.stop()


# In[26]:


d = monitor(controllers, duration)
freq_plot(d)

results['without primitive'] = d


# In[27]:


follow_trajectory(motor)


# ### Without all sensors

# In[28]:


for s in robot.sensors:
    s.close()


# In[29]:


d = monitor(controllers, duration)
freq_plot(d)

results['without sensor'] = d


# In[30]:


follow_trajectory(motor)


# In[ ]: