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

# #Poppy Torso interacting with objects in V-REP using Pypot
# 
# We suppose here that you've read the tutorial about [Controlling a Poppy Humanoid](http://nbviewer.ipython.org/github/poppy-project/pypot/blob/master/samples/notebooks/Controlling%20a%20Poppy%20humanoid%20in%20V-REP%20using%20pypot.ipynb), and will reuse the pypot library to control a Poppy robot, so you can refer to that tutorial if you need more details about it. 
# 
# In this tutorial, we will illustrate the use of the Poppy Torso creature in the V-REP simulator, interacting with objects.

# ## Installation
# As with the Poppy Humanoid creature, you can install the Torso one with the following pip command:
# 
# pip install poppy_torso
# 
# and check your installation with

# In[1]:


from pypot.creatures import PoppyTorso


# Here we suppose you have started a V-REP simulator, and we instantiate a simulated Torso robot:

# In[2]:


poppy = PoppyTorso(simulator='vrep')


# <img src="image/poppy_torso.png" alt="V-REP header" style="height: 400px;"/>

# ## Interaction with objects
# 
# Now we can use the robot's io interface to pop some objects.

# In[3]:


io = poppy._controllers[0].io


# Let's create a cube in front of the robot:

# In[4]:


name = 'cube'
position = [0.2, 0, 0.8] # X, Y, Z
sizes = [0.1, 0.1, 0.1] # in meters
mass = 0.5 # in kg
io.add_cube(name, position, sizes, mass)


# <img src="image/torso_cube.png" alt="V-REP header" style="height: 400px;"/>

# The objects can be moved to a given position, provided you know the object's name in V-REP. 
# 
# Let's try with this cube called 'cube':

# In[5]:


io.set_object_position('cube', [0.15,0.05,0.8])


# <img src="image/cube_moved.png" alt="V-REP header" style="height: 400px;"/>

# Now the cube is close to the left hand of Poppy Torso, he wants to touch it:

# In[6]:


{m.name: m.present_position for m in poppy.motors}


# In[7]:


poppy.l_arm_z.goal_position = -30


# <img src="image/torso_move.png" alt="V-REP header" style="height: 400px;"/>

# The objects can be asked for their position and orientation at any time:

# In[8]:


io.get_object_position('cube')


# Here we see that Poppy has moved the cube from $X=15$ cm to $X= 25$ cm, and from $Y= 0$ cm to $Y= -5$ cm.
# 
# The position of the cube is defined as the position of its center.
# 
# Now for the orientation:

# In[9]:


io.get_object_orientation('cube')


# In[10]:


Z_orientation = io.get_object_orientation('cube')[2] * 180./3.14
Z_orientation


# Thus the cube has then been rotated around the $Z$ axis by $0.06$ rad or $3.7$ deg.

# ## Different objects
# Different types of objects are readily available: cuboids, spheroids, cylinders and cones, using a similar API.
# 
# Let's add a ball, a cylinder and a cone:

# In[11]:


io.add_sphere('ball1', [0.1, 0, 0.8], [0.1, 0.1, 0.2], 2)


# In[12]:


io.add_cylinder('cylinder1', [0.1, 0, 0.8], [0.1, 0.1, 0.2], 2, [1000,1000])


# In[40]:


io.add_cone('cone1', [0.1, 0, 0.8], [0.1, 0.1, 0.2], 2, [1000,1000])


# For those 3 types of objects, an optional parameter can be set: $precision$ (default = [10, 10]).
# 
# The precision represents the number of edges of the object along the 2 dimensions of its surface.

# Notes: 
#     * you should avoid to pop 2 objects with the same name,
#     * spheroids, cylinders and cones seems not to be very stable in V-REP.

# <img src="image/objects.png" alt="V-REP header" style="height: 400px;"/>

# ## Bonus
# You can play with the for loop...

# In[13]:


from IPython.display import VimeoVideo
VimeoVideo(127023576)


# In[ ]: