In this notebook, we will present how a simulated Poppy Humanoid - an open-source and 3D printed humanoid robot - can be controlled in real time. The robot will be simulated in V-REP a well known and powerful robot simulator. In this tutorial we will show how to install, use, and program the simulated robot in Python. To do that, we will use the pypot library developed to easily control and program Poppy Creatures.
The tutorial has been tested on Mac and Linux but should also worked on Windows (any feedback from Windows users will be appreciated ;)). While the notebook has been written in Python 2.7, pypot is known to also work on Python 3.4 and PyPy-2.5.
In more details, we will:
Note: Most of the tutorial is redundant with the ones on how to control a "real" poppy creature. In particular, switching from a real robot to a simulated one (and vice versa) can be done just by changing a single line of code (see the appendix at the end of this notebook). Furthermore, most of the notebook can be applied to any Poppy Creature (and even any "pypot robot"), only the instantiation method will change.
Comments, issues, improvements and updates can be sent directly on the dedicated section of the github issue tracker.
First, if you do not know how to run an IPython Notebook please refer to our readme.
To follow this tutorial you will need:
Both V-REP and the pypot/poppy libraries are open source and cross platform.
The pypot and poppy_humanoid library can be installed via pip - a tool for installing Python Package (if you have no idea what pip is or how to run the following command, please refer to our readme first :-)):
pip install pypot poppy_humanoid
You can also install them from the source and then use the classical:
python setup.py install
Note: installing poppy_humanoid will also install pypot as it is one of the depencies.
To check if everything is installed correctly, you can run the following code. If it runs without raising an error, everything is probably installed correctly:
You can run IPython Notebook code cells by selecting them and clicking the play button or by pressing shift+enter.
In [1]:
from pypot.vrep import from_vrep
from poppy.creatures import PoppyHumanoid
If you have any installation issue, please check our forum or the github issue tracker.
In this section, we will see how a Poppy Humanoid can be created into V-REP and how we can connect it to a pypot Robot: i.e. the object used in pypot to represent and communicate with a robot.
First, you will need to launch V-REP (please refer to V-REP documentation if you don't know how to do it). Once it's done you should see something like:
Instead of loading a specific scene with a Poppy humanoid through the V-REP GUI and then connect to it using pypot, we will directly instantiate the PoppyHumanoid class which will do most of the work for us.
In particular, it will:
To do that, we will use the following code:
In [2]:
from poppy.creatures import PoppyHumanoid
poppy = PoppyHumanoid(simulator='vrep')
You should now see a Poppy in your V-REP window:
Note: Be careful that VREP is often displaying pop-up that freezes the communication with pypot. You will have to close them otherwise a timeout will occur!
As soon as you have instantiated a Robot - in our case through the PoppyHumanoid class - it is synced with the simulation (or the real robot). This means that values from the V-REP simulation (e.g. limbs position) are retrieved from the simu and affected to their equivalent variables by a synchronization loop. Similarly target variables (e.g. motors goal position) are sent to V-REP. This synchronization loop runs at 50Hz by default.
To be more clear, when reading a variable from the poppy object you will obtain the last synced value from V-REP and when setting a new value to a poppy variable it will be automatically sent to V-REP a short time after. You never need to manually sync your instance with the current state of the simulation, it is automatically done by a thread running in background.
Dynamixel motors comes with a lot of registers which are used to store the current state of the robot (its current position, temperature, pid gains...) but also where you can write new target values, for instance a new goal position.
In this section we will see how pypot give you an high-level access to the most frequently used registers (pypot low-level IO gives you an access to all registers but this is beyond the scope of this tutorial).
So, first we will retrieve the list of all available motors. The motors variable contains the list of all motors attached to the current robot.
By default, each motor prints its name, its id, and its current position:
In [3]:
poppy.motors
Out[3]:
You can access a specific motor directly using its name:
In [4]:
poppy.l_shoulder_y
Out[4]:
If we want to get the current position (in degrees) of a specific motor (e.g. head_y) we can use:
In [5]:
poppy.head_y.present_position
Out[5]:
You can also use the list/dict comprehension to retrieve a specific value for all motors.
A list of all current motor positions:
In [6]:
[m.present_position for m in poppy.motors]
Out[6]:
A dictionary of pairs {motor_name: motor_position}:
In [7]:
{m.name: m.present_position for m in poppy.motors}
Out[7]:
In pypot we use the concept of motor alias which is simply a list of motors grouped together under a specific name. For instance, you can directly access all the motors from the torso using the torso alias. Poppy Humanoid also defines a leg alias, a left arm alias...
Note, that motors used above is just one of the predefined motors alias - one with all attached motors.
You can retrieve the list of motors alias available using:
In [8]:
poppy.alias
Out[8]:
Each alias contains a list of motors. Thus, you can similarly retrieve all positions for only the motors of the right leg:
In [9]:
{m.name: m.present_position for m in poppy.r_leg}
Out[9]:
In a similar way that you retrieve values from V-REP, you can set a new target position to a motor.
By sending the following command, you should see the robot turns its head of 90°:
In [10]:
poppy.head_z.goal_position = 90.
Or you can affect new target positions for a group of motors:
In [11]:
for m in poppy.l_arm:
m.goal_position = 30.
It's important to note the difference between the current and goal position. In particular, when setting a new goal position, it will take time before the motor actually reaches the desired position (see section below for an example).
Thus, in the code below only the second instruction will likely have an effect on the robot:
In [12]:
poppy.r_shoulder_x.goal_position = 30
poppy.r_shoulder_x.goal_position = -30
Note: While the full list of motor registers is available, not all of them are having an effect in the V-REP simulation. For instance, modifying the pid of a motor won't affect the simulation.
Currently in the V-REP simulator you can use:
Support for additional features may be added in future version.
You can also use the goto_position method (both at the robot or motor level) to get more control over the trajectory of a motor. In the examples above, when affecting the goal_position the motor will try to reach it as fast as the moving_speed permits it.
At the moment, goto_position comes with two behaviors:
First, let's restart the simulation:
In [13]:
poppy.reset_simulation()
Now, we make the head move towards -45° in 2 seconds:
In [14]:
poppy.head_z.goto_position(-45, 2)
Goto position also comes with a wait arguments, so you can easily link motions (wait=True will wait for the movement to finish before executing the next line, while wait=False will send the new target position order and directly jump to the next instruction):
In [15]:
poppy.head_z.goto_position(45, 2, wait=False)
poppy.head_y.goto_position(-30, 2, wait=True)
poppy.head_z.goto_position(0, 2, wait=True)
poppy.head_y.goto_position(20, 1, wait=True)
You can get and set a new goto_behavior through the property:
In [16]:
poppy.head_y.goto_behavior
Out[16]:
In [17]:
poppy.head_y.goto_behavior = 'dummy'
Let's prepare another example where we will illustrate the difference between present and goal position by applying a sinusoid on a specific motor.
To make sure the robot is in a stable position, we will reset the simulation. This will re-positioned the robot in its initial position:
In [18]:
poppy.reset_simulation()
Now let's make the robot's head moves:
In [19]:
import time
import math
amp = 30 # in degrees
freq = 0.5 # in Hz
t0 = time.time()
while True:
t = time.time()
# run for 10s
if t - t0 > 10:
break
poppy.head_z.goal_position = amp * math.sin(2 * 3.14 * freq * t)
time.sleep(0.04)
Now we will use the same code but we will record both the current and goal position:
In [20]:
current, goal = [], []
t0 = time.time()
while True:
t = time.time()
# run for 5s
if t - t0 > 5:
break
poppy.head_z.goal_position = amp * math.sin(2 * 3.14 * freq * t)
current.append(poppy.head_z.present_position)
goal.append(poppy.head_z.goal_position)
time.sleep(0.04)
If we plot the two trajectories, we can clearly see a time shift representing the time needed by the motor to reach the desired position:
In [21]:
%pylab inline
t = linspace(0, 5, len(current))
plot(t, goal)
plot(t, current)
legend(('goal', 'current'))
Out[21]:
Similarly, we can observe a goto position using the minimum jerk mode which shows the smooth acceleration and deceleration:
In [22]:
poppy.l_shoulder_x.goto_behavior = 'minjerk'
poppy.l_shoulder_x.goto_position(120, 5)
pos = []
t0 = time.time()
while time.time() - t0 < 5:
pos.append(poppy.l_shoulder_x.present_position)
time.sleep(0.01)
t = linspace(0, 5, len(pos))
plot(t, pos)
Out[22]:
In [23]:
poppy.reset_simulation()
Using a V-REP simulated robot, you can easily retrieve an object position and orientation. You just need to know its name in the vrep scene.
Note: at the moment to know the name of object in the vrep scene, you have to look for them in the v-rep window. Hopefully in future version of pypot, you will be able to directly retrieve them.
For instance, to get the 3D position of the left hand, you just have to do:
In [24]:
poppy.get_object_position('l_forearm_visual')
Out[24]:
By default, the position is in the V-REP scene referential (the zero is somewhere between Poppy Humanoid's feet). You can use any object as referential and thus get the left forearm position related to the head for instance:
In [25]:
poppy.get_object_position('l_forearm_visual', 'head_visual')
Out[25]:
This can be used for discovering a reachable space for instance:
In [26]:
reached_pt = []
for m in poppy.l_arm:
m.goto_behavior = 'minjerk'
# We generate 25 random arm configuration
# and stores the reached position of the forearm
for _ in range(25):
poppy.reset_simulation()
# Generate a position by setting random position (within the angle limit) to each joint
# This can be hacked to define other exploration
pos = {m.name: randint(min(m.angle_limit), max(m.angle_limit)) for m in poppy.l_arm}
poppy.goto_position(pos, 2., wait=True)
reached_pt.append(poppy.get_object_position('l_forearm_visual'))
In [27]:
from mpl_toolkits.mplot3d import Axes3D
ax = axes(projection='3d')
ax.scatter(*array(reached_pt).T)
Out[27]:
This example could be extended to show a simple method to build an inverse model (you build a table with many goals in the search space associated with the motor command which generated it, and for the inverse model you reproduce the motor command of the stored goal closest to the point you want to reach).
This could be a very good exercise where in a specific notebook you describe a simple approach to build and use approximated inverse models.
If you are interested in this kind of experiments and want to go further, you can see the explauto library. It provides a unified framework for autonomous exploration experiment notably using a Poppy Creature. You will find there learning algorithms that can be used to learn forward (e.g. where the end position of an arm is depending on each joints position) and inverse model (finding the joint angles to locate the end effector at a desired position).
In [28]:
poppy.reset_simulation()
Pypot also comes with the Primitive abstraction. The idea is to write simple behaviors that can be automatically combined to create more complex behaviors. As the primitive is likely to change in the future, and go toward something better defined, we will only show the very basic stuff you can do using primitives.
In more technical details, a primitive is only a thread which have access to all robot sensors and effectors. A primitive manager is used to gather all primitives orders and combined them using a filter (a simple sum by default).
As an example of how to use primitives, we will use one of the predefined primitives, the Sinus:
In [29]:
from pypot.primitive.utils import Sinus
Primitives are usually instantiated with a robot as the first argument. As Sinus is a LoopPrimitive (i.e. a specific primitive which call an update method at a predefined frequency), you also need to pass the call frequency as the second argument).
The other arguments, here the motors list, the amplitude and the frequency are specific to the Sinus primitive.
In [30]:
sin_1 = Sinus(poppy, 25., [poppy.head_z, poppy.head_y], amp=15, freq=.15)
A primitive can be:
By running the following code, you should see both motors of the head performs a sinusoid.
In [31]:
sin_1.start()
Multiples primitives can be runned at the same time:
In [32]:
sin_2 = Sinus(poppy, 25., [poppy.head_z, ], amp=8, freq=.5)
sin_2.start()
We will now write a simple motor position logger using a loop primitive:
In [33]:
from pypot.primitive import LoopPrimitive
class MotorPositionLogger(LoopPrimitive):
def __init__(self, robot, refresh_freq, motor):
LoopPrimitive.__init__(self, robot, refresh_freq)
self.motor = getattr(self.robot, motor.name)
# This code will be called each time the primitive is started
def setup(self):
self.positions = []
# This method will be called at the predefined frequency
def update(self):
self.positions.append(self.motor.present_position)
In [34]:
logger = MotorPositionLogger(poppy, 50, poppy.head_z)
We will illustrate the combination of primitives by pausing one of them in the middle of the recording:
In [35]:
logger.start()
time.sleep(10)
sin_2.pause()
time.sleep(15)
sin_2.resume()
time.sleep(5)
logger.stop()
pos = logger.positions
plot(linspace(0, 30, len(pos)), pos)
Out[35]:
You can see on the plot above, that the two sinusoids are combined from 0 to 10 and from 25 to 30. From 10 to 25 only one of the sinusoid is applied.
Now we stop all running primitives:
In [36]:
for p in poppy.active_primitives:
p.stop()
As mentionned in the introduction, most of the code above can be run on a "real" robot. All you need to change is the way the robot is instantiated.
You have to replace:
poppy = PoppyHumanoid(simulator='vrep')
by
poppy = PoppyHumanoid()
Of course, some of the directly V-REP related code - such as reset_simulation or get_object_position does not have any direct equivalent on the real robot. But all the rest should work and give about the same results on the real or the simulated robot.