[Note: as pypot permits to transparently switch from the real robot to the simulated version, this notebook is really similar to the one used to control the real robot]
Explauto comes with a PyPotEnvironment which links the pypot library to the explauto framework. This library, also developed in the Flowers team can be used to control robot based on the dynamixel motors. It is notabely used in the poppy-project to control Poppy. This framework also allows to use simulated version of the robot thanks to the V-REP software.
As an example of how explauto can be used with pypot, we will setup a simple experiment where Poppy will learn the inverse model of its arm. To illustrate the power of explauto, we will make it learn simultaneously on both arms, by using motor babbling on the left arm and goal babbling on the right arm.
We assume here that you are already familiar with pypot, or at least that you know the very basic stuff. If it is not the case you should directly refer to Poppy documentation. Note that in order to work with the PypotEnvironment you need to have installed pypot first. To use it with Poppy you also need the poppytools. If you do not have setup pypot with V-REP yet, you should refer to this HowTo.
First, Launch V-REP :-)
And then instantiate your robot.
In [1]:
import json
from pypot.vrep import from_vrep
config_path = 'poppy_config.json'
scene_path = 'poppy-sitting.ttt'
with open(config_path) as cf:
config = json.load(cf)
poppy = from_vrep(config, '127.0.0.1', 19997, scene_path,
tracked_objects=['left_hand_tracker', 'right_hand_tracker'])
Starts the synchronization loops.
In [2]:
poppy.start_sync()
If everything went well, you should see something like this.
The PypotEnvironment is instantiated with:
In [3]:
from explauto.environment.pypot import PypotEnvironment
from explauto.environment import environments
pypot_env_configs = {}
We first define both arms configuration:
In [4]:
from numpy import deg2rad, array
# motor bounds for the left arm
l_m_mins = deg2rad(array([-15, 0, -90, -90]))
l_m_maxs = deg2rad(array([90, 90, 90, 0]))
# motor bounds for the right arm
r_m_mins = deg2rad(array([-15, -90, -90, -90]))
r_m_maxs = deg2rad(array([90, 0, 90, 0]))
# sensor bounds for the left arm
l_s_mins = array((-0.2, -0.1, 0.0))
l_s_maxs = array((0.4, 0.5, 0.6))
# sensor bounds for the right arm
r_s_mins = array((-0.2, -0.5, 0.0))
r_s_maxs = array((0.4, 0.1, 0.6))
We define a simple tracker using V-REP object.
In [5]:
class VrepTracker(object):
def get_position(self, tracked_object):
return getattr(poppy, tracked_object).position
tracker = VrepTracker()
Then, let's define the config for the left arm.
In [6]:
env_cls = PypotEnvironment
conf = {'pypot_robot': poppy,
'motors': poppy.l_arm,
'move_duration': 1.0,
'tracker': tracker,
'tracked_obj': 'left_hand_tracker',
'm_mins': l_m_mins,
'm_maxs': l_m_maxs,
's_mins': l_s_mins,
's_maxs': l_s_maxs}
pypot_env_configs['l_arm'] = conf
Then, the right arm config:
In [7]:
conf = {'pypot_robot': poppy,
'motors': poppy.r_arm,
'move_duration': 1.0,
'tracker': tracker,
'tracked_obj': 'right_hand_tracker',
'm_mins': r_m_mins,
'm_maxs': r_m_maxs,
's_mins': r_s_mins,
's_maxs': r_s_maxs}
pypot_env_configs['r_arm'] = conf
And finally we register them both:
In [8]:
environments['poppy'] = (PypotEnvironment, pypot_env_configs, None)
In [9]:
from explauto.experiment import Experiment, make_settings
In [10]:
s = make_settings(environment='poppy',
babbling_mode='motor',
interest_model='random',
sensorimotor_model='nearest_neighbor',
environment_config='l_arm')
motor_xp = Experiment.from_settings(s)
s = make_settings(environment='poppy',
babbling_mode='goal',
interest_model='random',
sensorimotor_model='nearest_neighbor',
environment_config='r_arm')
goal_xp = Experiment.from_settings(s)
In [11]:
N_ITER = 100
In [12]:
for _ in range(N_ITER):
goal_xp.run(1, bg=True)
motor_xp.run(1, bg=True)
goal_xp.wait() and motor_xp.wait()
poppy.reset_simulation()
In [13]:
%pylab inline
In [14]:
ax_motor = subplot(121)
ax_motor.axis([0, 1, -1, 1])
data = motor_xp.log.scatter_plot(ax_motor, (('sensori', [0, 1]), ), color='red')
legend(('motor', ))
ax_goal = subplot(122)
ax_goal.axis([0, 1, -1, 1])
data = goal_xp.log.scatter_plot(ax_goal, (('sensori', [0, 1]), ), color='green')
legend(('goal', ))
Out[14]:
In [ ]: