<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 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
Step1:  All bench info will be stored in this dictionary so it's easy to compare with other platforms.
Step2:  What's the platform
Step3:  Make sure all motors are turned off to avoid breaking anything
Step4:  We find the synchronization loop for pos/speed/load and monkey patch them for monitoring.
Step5:  Now, we define our monitor and plotting functions.
Step6:  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
Step7:  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.
Step8:  Benchmark
Our benchmark duration in seconds
Step9:  Normal usage
Step10:  Without primitives
Step11:  Without all sensors
<ASSISTANT_TASK:>
Python Code:
from __future__ import print_function, division
from ipywidgets import interact
%pylab inline
Explanation: 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
End of explanation
results = {}
Explanation: All bench info will be stored in this dictionary so it's easy to compare with other platforms.
End of explanation
import platform
p = platform.platform()
print(p)
results['platform'] = p
import sys
v = sys.version
print(v)
results['python'] = v
import pypot
import poppy.creatures
results['pypot'] = pypot.__version__
print('Pypot version: {}'.format(results['pypot']))
results['poppy-creature'] = poppy.creatures.__version__
print('Poppy-creature version: {}'.format(results['poppy-creature']))
from poppy.creatures import installed_poppy_creatures
RobotCls = None
def robot_selector(robot):
    global RobotCls
    RobotCls = robot
    
interact(robot_selector, robot=installed_poppy_creatures);
robot = RobotCls()
results['robot'] = RobotCls
Explanation: What's the platform
End of explanation
for m in robot.motors:
    m.compliant = True
Explanation: Make sure all motors are turned off to avoid breaking anything:
End of explanation
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)]
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()
Explanation: We find the synchronization loop for pos/speed/load and monkey patch them for monitoring.
End of explanation
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 {}: {}ms STD={}ms'.format(c.ids, freq.mean(), freq.std()))
        hist(freq)
        xlim(0, 100)
Explanation: Now, we define our monitor and plotting functions.
End of explanation
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)    
Explanation: 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:
End of explanation
motor = None
def motor_selector(m):
    global motor
    motor = getattr(robot, m)
    
interact(motor_selector, m=[m.name for m in robot.motors]);
Explanation: 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.
End of explanation
duration = 30
Explanation: Benchmark
Our benchmark duration in seconds:
End of explanation
d = monitor(controllers, duration)
freq_plot(d)
results['normal'] = d
follow_trajectory(motor)
Explanation: Normal usage
End of explanation
for p in robot.primitives:
    p.stop()
    
robot._primitive_manager.stop()
d = monitor(controllers, duration)
freq_plot(d)
results['without primitive'] = d
follow_trajectory(motor)
Explanation: Without primitives
End of explanation
for s in robot.sensors:
    s.close()
d = monitor(controllers, duration)
freq_plot(d)
results['without sensor'] = d
follow_trajectory(motor)
Explanation: Without all sensors
End of explanation
<END_TASK>