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

# In[1]:


from poppy.creatures import PoppyHumanoid

poppy = PoppyHumanoid(simulator='vrep',scene='poppy_humanoid_inertia.ttt')

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


# Prepared poppy for experiments

# In[2]:


poppy.head_y.goto_position(0, 4, wait=False)
poppy.r_shoulder_y.goto_position(0, 4, wait=False)
poppy.l_shoulder_y.goto_position(0, 4, wait=True)
poppy.l_shoulder_x.goto_position(20, 4, wait=False)
poppy.r_shoulder_x.goto_position(-20, 4, wait=True)


# Put poppy's hands in the air and plot the movement

# In[4]:


time.sleep(1)
poppy.l_shoulder_x.goal_position = 120
poppy.r_shoulder_x.goal_position = -120
pos = []
pos2 = []
t = []
t2 = []
t0 = time.time()
while time.time()-t0 <3:
    t_simu = poppy.current_simulation_time
    time.sleep(0.01)
    if poppy.current_simulation_time != t_simu:
        
        pos.append(poppy.l_shoulder_x.present_position - poppy.l_shoulder_x.goal_position)
        t.append(poppy.current_simulation_time)
        pos2.append(poppy.r_shoulder_x.present_position - poppy.r_shoulder_x.goal_position)
        t2.append(poppy.current_simulation_time)
    
print poppy.r_shoulder_x.present_position - poppy.r_shoulder_x.goal_position
print poppy.l_shoulder_x.present_position - poppy.l_shoulder_x.goal_position
plot(t,pos)
plot(t2,pos2)


# In[3]:





# In[ ]: