Controlling an Ergo-Robot with the Leap Motion

To run this code you will need to:

Connect to the Leap Motion and calibrate

In [1]:
import Leap

In [3]:
from leap_listener import SampleListener

In [4]:
# import sys

listener = SampleListener()
controller = Leap.Controller()

# Have the sample listener receive events from the controller


Calibration part

Run the cell below and move your hand above the Leap Motion during 5 seconds for calibration. You have to cover the range of 3D hand position you want to use in order to control the robot.

In [5]:
values = []
import time
for _ in range(1000):


In [6]:
from numpy import array
values = array(values)

In [7]:
from numpy import min, max
mins = min(values, axis=0)
maxs = max(values, axis=0)
print mins, maxs

[-197.75019836   72.81723785 -110.87555695] [ 158.55618286  452.26123047  150.54743958]

Connect to the Ergo-Robot

The code below use the 'ergo.json' configuration file. Please check if it is compatible with your robot version.

In [8]:
from pypot.robot import from_json

ergo = from_json('ergo.json')

Limit moving speed and torque limit to avoid a dispute with your robot:

In [9]:
for m in ergo.motors:
    m.moving_speed = 80
ergo.compliant = False

for m in ergo.motors:
    m.torque_limit = 80

WARNING: The code code below will move the robot to a neutral position:

In [10]:
for m in ergo.motors:
    m.goal_position = 0

Create a Pypot primitive to link Leap Motion tracking to robot movements:

In [ ]:
from pypot.primitive import LoopPrimitive
#from IPython.display import display, clear_output

class LeapPrimitive(LoopPrimitive):

    """ Apply a sinus on the motor specified as argument. Parameters (amp, offset and phase) should be specified in degree

    def __init__(self, robot, refresh_freq, leap_listener):

        LoopPrimitive.__init__(self, robot, refresh_freq)
        self.leap_listener = leap_listener

    def update(self):
        """ Compute the sin(t) where t is the elapsed time since the primitive has been started. """
            hand_coord = self.leap_listener.hand_coord
            # hand_coord = self.leap_listener.hand_coord_norm
            grip_size = self.leap_listener.grip_size
            for i in range(3):
                pos = (hand_coord[i] - mins[i]) / (maxs[i] - mins[i])
                # pos = hand_coord[i]
                if i == 1:
                    pos = 1 - pos
                pos -= 0.5
                pos /= 4
                pos += 0.5
                if i == 2:
                    motor = self.robot.base[-1]
                    motor = self.robot.head[i]
                pos = pos * (motor.angle_limit[1] - motor.angle_limit[0]) + motor.angle_limit[0]

                motor.goal_position = pos
            motor = self.robot.gripper
            self.robot.gripper.goal_position = grip_size * (motor.angle_limit[1] - motor.angle_limit[0]) + motor.angle_limit[0]

Attach the primitive to your robot and run it:

In [11]:
prim = LeapPrimitive(ergo, 50, listener)
ergo.attach_primitive(prim, 'leap')

Let's play!

Now you can move your hand above the Leap Motion and control your robot in this way. You can for example try to grasp a plastic glass with it (only