TODO:
In this notebook, we will present how a simulated poppy humanoid can be controlled in real time using pypot.
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).
To follow this tutorial you will need:
If the following code runs without raising an error, everything is probably installed correctly:
from pypot.vrep import from_vrep
In this section, we will see how a Poppy Humanoid can be instantiated into V-REP and how we can connect it to a pypot 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:
In particular, it will:
To do that, we will use the following code:
import os
import json
import pypot
import poppytools
from pypot.vrep import from_vrep, close_all_connections
# We first close all previously opened connection(s) to ensure that everything is clean and ready.
close_all_connections()
# Load the robot configuration from a json file. This dictionary contains all the motors name, their alias...
with open(os.path.join(os.path.dirname(poppytools.__file__), 'configuration', 'poppy_config.json')) as f:
robot_config = json.load(f)
# Define the path to a V-REP scene which contains a poppy humanoid in a sitting position
scene = os.path.join(os.path.dirname(pypot.__file__), '..', 'samples', 'notebooks', 'poppy-sitting.ttt')
# Instantiate a pypot Robot and connect it to the simulated robot
poppy = from_vrep(robot_config, scene=scene)
You should now see a Poppy in your V-REP window:
Note: as poppy will soon be directly integrated in V-REP, this process should become even simpler. Something like the code below should soon become possible:
from pypot.vrep import from_vrep
poppy = from_vrep('Poppy-Humanoid')
As soon as you have instantiated a Robot using the from_vrep function, it is synced with the simulation. This means that values from the V-REP simulation (e.g. limbs position) are retrieved and affected to their equivalent variables. 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.
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 and current position:
poppy.motors
[<DxlMotor name=l_elbow_y id=44 pos=0.0>, <DxlMotor name=r_elbow_y id=54 pos=-0.0>, <DxlMotor name=r_knee_y id=24 pos=120.1>, <DxlMotor name=head_y id=37 pos=-0.0>, <DxlMotor name=head_z id=36 pos=0.2>, <DxlMotor name=r_arm_z id=53 pos=0.0>, <DxlMotor name=r_ankle_y id=25 pos=-70.2>, <DxlMotor name=r_shoulder_x id=52 pos=-0.2>, <DxlMotor name=r_shoulder_y id=51 pos=0.6>, <DxlMotor name=r_hip_z id=22 pos=-1.1>, <DxlMotor name=r_hip_x id=21 pos=-19.2>, <DxlMotor name=r_hip_y id=23 pos=-25.0>, <DxlMotor name=l_arm_z id=43 pos=0.0>, <DxlMotor name=l_hip_x id=11 pos=15.2>, <DxlMotor name=l_hip_y id=13 pos=-25.0>, <DxlMotor name=l_hip_z id=12 pos=0.2>, <DxlMotor name=abs_x id=32 pos=0.0>, <DxlMotor name=abs_y id=31 pos=-0.0>, <DxlMotor name=abs_z id=33 pos=-0.0>, <DxlMotor name=l_ankle_y id=15 pos=-70.4>, <DxlMotor name=bust_y id=34 pos=0.0>, <DxlMotor name=bust_x id=35 pos=-0.0>, <DxlMotor name=l_knee_y id=14 pos=119.0>, <DxlMotor name=l_shoulder_x id=42 pos=-9.9>, <DxlMotor name=l_shoulder_y id=41 pos=0.0>]
If we want to get the current position (in degrees) of a specific motor (e.g. head_z) we can use:
poppy.head_z.present_position
0.2
Each motor can be accessed directly using its name:
poppy.l_shoulder_x
<DxlMotor name=l_shoulder_x id=42 pos=-9.9>
Or you can use the list/dict comprehension to retrieve a specific value for all motors.
A list of all current motor positions:
[m.present_position for m in poppy.motors]
[0.0, -0.0, 120.1, -0.0, 0.2, 0.0, -70.2, -0.2, 0.6, -1.1, -19.1, -25.0, 0.0, 15.1, -25.0, 0.1, 0.0, -0.0, -0.0, -70.4, 0.0, -0.0, 119.0, -9.9, -0.0]
A dictionary of pairs {motor_name: motor_position}:
{m.name: m.present_position for m in poppy.motors}
{u'abs_x': 0.0, u'abs_y': -0.0, u'abs_z': -0.0, u'bust_x': -0.0, u'bust_y': 0.0, u'head_y': -0.0, u'head_z': 0.2, u'l_ankle_y': -70.4, u'l_arm_z': 0.0, u'l_elbow_y': 0.0, u'l_hip_x': 15.0, u'l_hip_y': -25.0, u'l_hip_z': 0.2, u'l_knee_y': 119.0, u'l_shoulder_x': -9.9, u'l_shoulder_y': -0.0, u'r_ankle_y': -70.2, u'r_arm_z': 0.0, u'r_elbow_y': -0.0, u'r_hip_x': -19.0, u'r_hip_y': -25.0, u'r_hip_z': -1.2, u'r_knee_y': 120.1, u'r_shoulder_x': -0.2, u'r_shoulder_y': 0.6}
The motor variable is just one of the predefined motor alias - one with all attached motors. Poppy Humanoid also defines a leg alias, a left arm alias...
You can retrieve the list of motor alias available using:
poppy.alias
[u'r_leg', u'torso', u'l_leg_sagitall', u'head', u'l_arm', u'r_leg_sagitall', u'l_leg', u'arms', u'legs', u'r_arm']
Each alias contains a list of motors. Thus, you can similarly retrieve all positions for only the motors of the right leg:
{m.name: m.present_position for m in poppy.r_leg}
{u'r_ankle_y': -70.2, u'r_hip_x': -19.0, u'r_hip_y': -25.0, u'r_hip_z': -1.1, u'r_knee_y': 120.1}
In a similar way, you can set a new target position to a motor.
By sending the following command, you should see the robot turns its head:
poppy.head_z.goal_position = 90.
Or you can affect new target positions for a group of motors:
for m in poppy.l_arm:
m.goal_position = 45.
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:
poppy.abs_z.goal_position = -45
poppy.abs_z.goal_position = 45
Note: While the full list of motor registers is available, most of them are not producing any effect in the V-REP simulation. For instance, modifying the pid of a motor won't affect the simulation. Support for additional features may be added in future version.
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 "correct" position, we will reset the simulation. This will re-positioned the robot in its initial position:
poppy.reset_simulation()
Now let's make the robot's head moves:
import time
from math import sin
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 * sin(2 * 3.14 * freq * t)
time.sleep(0.1)
Now we will use the same code but we will record both the current and goal position:
current, goal = [], []
t0 = time.time()
while True:
t = time.time()
# run for 5s
if t - t0 > 5:
break
poppy.head_z.goal_position = amp * sin(2 * 3.14 * freq * t)
current.append(poppy.head_z.present_position)
goal.append(poppy.head_z.goal_position)
time.sleep(0.1)
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:
%pylab inline
t = linspace(0, 5, len(current))
plot(t, goal)
plot(t, current)
legend(('goal', 'current'))
Populating the interactive namespace from numpy and matplotlib
<matplotlib.legend.Legend at 0x10ac50f90>