Logo

SelectScript_OpenRAVE

1. Introduction

With SelectScript we wanted to implement a general interface for various simulation environments. Retrieving data, information, and also abstractions from a simulation environment should be as easy as querying a database with SQL, without paying attention to the underlying interfaces ... The following video shall give a little impression about the usage of SelectScript. This version, called SelectScript_OpenRAVE, was especially developed for OpenRAVE, but as we will show, it is easy to adapt the language and to integrate new functionalities ... Because our query language is under steady development, we will adapt also this tutorial step by step to allow more insights...

In [1]:
from IPython.display import YouTubeVideo
YouTubeVideo('R_PThP0gwOc', width=640, height=430)
Out[1]:

1. Contact

If you have comments, suggestions, hints, etc. (or even problems), feel free to contact us.

André Dietrich, dietrich@ivs.cs.uni-magdeburg.de, http://eos.cs.ovgu.de/dietrich/

Sebastian Zug, zug@ivs.cs.uni-magdeburg.de, http://eos.cs.ovgu.de/zug/

2. Installation

This extension of SelectScript for OpenRAVE was implemented in Python2.7 and with the help of ANTLR3 (everything was tested on Ubuntu 12.04). There are different ways to install SelectScript_OpenRAVE, but first of all you will have to install OpenRAVE itself on your system, follow the installation instructions at http://openrave.org/docs/latest_stable/install/#install, we recommend to use the latest stable version 0.9.0. Afterwards follow one of the 3 steps listed below:

2.1 Python-Pip

We recommend the usage of the python install tool (pip), which automatically downloads and installs all required sources and dependencies.

pip install SelectScript_OpenRAVE

2.2 Tar.Gz

Download the ta.gz from: https://pypi.python.org/packages/source/S/SelectScript_OpenRAVE/SelectScript_OpenRAVE-1.tar.gz

and install it manually with: python setup.py install

2.3 Subversion

Checkout our project from http://sourceforge.net/projects/selectscript/

svn checkout svn://svn.code.sf.net/p/selectscript/code/trunk/ selectscript

This will download the whole implementation of SelectScript containing the base implementation, grammar files, and other derived implementations for different simulations, etc. Afterwards install the packages according to section 2.2. This will be the most frequently updated version ...

2.4 First Run

We added some basic examples (demo0.py, demo1.py, demo2.py, demo3.py, demo4.py) to demonstrate and present the aspects of our approach. All the demos can be started with the following 2 lines of code ... (you should see a running simulation at this point, or something with the installation went wrong)

In []:
from SelectScript_OpenRAVE.examples import demo0
demo0.start()

3. Initialization

To initialize all required modules run the following commands, which will load OpenRAVE, some further requirements as well as the SelectScript parser and the interpreter for OpenRAVE (ssRAVE).

In [2]:
from openravepy import *; import numpy, os
from SelectScript.SelectScript import SelectScript
import SelectScript_OpenRAVE.interpreter as ssRAVE

Initialize a new OpenRAVE environment and loud our kitchen-example:

In [3]:
env = Environment()     # create the environment
env.Load(os.path.dirname(ssRAVE.__file__)+"/examples/resources/kitchen.env.xml")  # load a model
Out[3]:
False

Afterwards initialize the SelectScript parser and then the interpreter. The parser is the same for every language implementation and generates some kind of simplified byte code, which is easier to interpret. But the interpreter itself has to be adapted, to work with different simulations environments; we will describe this later in more detail.

In [4]:
ssPar = SelectScript(None, None)
ssInt = ssRAVE.interpreter()

To start the visualization run the following command.

In [5]:
env.SetViewer('qtcoin') # start the viewer
Out[5]:
True

We will use an additional feature of OpenRAVE to place screenshots of the simulation into this notebook. The following code is only used for visualization purposes ...

In [6]:
%pylab inline
import matplotlib.pyplot as plt
plt.rcParams['figure.figsize'] = (10.0, 8.0)
def screenshot():
    env.GetViewer().SendCommand('SetFiguresInCamera 1')
    I = env.GetViewer().GetCameraImage(640,480,  env.GetViewer().GetCameraTransform(),[640,640,320,240])
    plt.imshow(I)
    plt.show()
Populating the interactive namespace from numpy and matplotlib

WARNING: pylab import has clobbered these variables: ['log']
`%matplotlib` prevents importing * from pylab and numpy

In [7]:
screenshot() # show a screenshot of the environment ...

4. Grammar

SelectScript is intended to be used for queries only, that is why you can call only externally defined functions (parameters have to be separated with commas), apply basic arithmetic and logic operations, store values in variables, and last but not least define queries (similar to SQL). Every expression has to end with a semicolon and parenthesis is supported.

In [8]:
expr = "1+2*3-4/5.;"       # expression is defined within a string, supported datatypes are bool, int, float, string, list
prog = ssPar.compile(expr) # some bytecode is generated
print ssInt.eval(prog)     # and interpreted ...
6.2

In [9]:
expr = """ 2 * 2;    // multible commands can defined within a script, comments are in C style ...
           a = False; """  # the last expression of a script defines the return value of the whole script
prog = ssPar.compile(expr)  
print ssInt.eval(prog)     
False

In [10]:
expr = """ b = (2+2)*3.45; 
           a = False or True;
           [b, a, "a string", 'or another string']; """  # use lists to define more complex return values ...
prog = ssPar.compile(expr)
print ssInt.eval(prog)
[13.8, True, u'a string', u'or another string']

Variables are stored persistently within the SelectScript interpreter and can also be called from outside with the following methods.

In [11]:
print ssInt.callVariable('b')
13.8

In [12]:
ssInt.addVariable("c", 1.234) # attach a new variable 
print ssInt.callVariable('c')
1.234

The next parts will describe how to use functions and of course, how to use queries and how to extend the language according to different requirements.

5. Get Help

Functions are mostly used to define relations, to get a first overview on available functions simply call help() the result is a list of all defined functions so far ...

In [13]:
expr = "help();"
prog = ssPar.compile(expr)
ssInt.eval(prog)
Out[13]:
['sensingEnvIDs',
 'help',
 'getTime',
 'within',
 'environmentID',
 'id',
 'isEnabled',
 'below',
 'to',
 'above',
 'var',
 'sensor',
 'isRobot',
 'isSensor',
 'pose',
 'volume',
 'position',
 'distance',
 'isKinbody',
 'obj',
 'kinbody',
 'type',
 'clear',
 'robot',
 'sensingAmount',
 'isVisible',
 'y',
 'x',
 'z',
 'isSensing']

To get help for a specific function-call use it in the following way.

In [14]:
expr = "help('isKinbody');"
prog = ssPar.compile(expr)
print ssInt.eval(prog)
isKinbody
Checks if object is of type Kinbody.
Usage: isKinbody(object) -> bool

In [15]:
expr = "help('obj');"
prog = ssPar.compile(expr)
print ssInt.eval(prog)
obj
Returns the object itself.
Usage: obj(object) -> object

6. First Queries

Before we can query the simulation environment, we have to add under a specific name to the interpreter. This is done in the same way as you would attach any other ordinary variable...

In [16]:
# add the environment as variable to the interpreter
ssInt.addVariable('kitchen', env)
Out[16]:
RaveGetEnvironment(1)

Starting with a simple query, where we might only interested in the positions and identifiers from entities within the kitchen environment that fulfill the property of being a robot. The result is given back as list of python dictionaries, similar to a table in databases.

In [17]:
expr = "SELECT id, position, type FROM kitchen WHERE isRobot(this);"
prog = ssPar.compile(expr)
for element in ssInt.eval(prog):
    print element['id'], ": \t",  numpy.round(element['position'],3), "\t", element['type']
fridge : 	[-0.87 -2.    1.55] 	<class 'openravepy._openravepy_.openravepy_int.Robot'>
cooker : 	[ 1.98  1.72  0.75] 	<class 'openravepy._openravepy_.openravepy_int.Robot'>
barrett_4918 : 	[ 0.42   1.3    0.892] 	<class 'openravepy._openravepy_.openravepy_int.Robot'>
roomba_625x : 	[ 1.8   0.    0.17] 	<class 'openravepy._openravepy_.openravepy_int.Robot'>

The return value of a SELECT query can be defined with the key word AS. Further supported abstractions are value and list, the python dict is default and does not have to be defined ...

In [18]:
expr = """// stores the table objects within a variable
          table = SELECT obj FROM kitchen WHERE id(this) == 'table' AS value;

          // select all volumes (AS list) of objects that are above the object 
          // table an with an volume smaller than 0.001
          SELECT volume 
          FROM kitchen 
          WHERE above(table, this) and volume(this) < 0.001 
          AS list; 
       """ 
prog = ssPar.compile(expr)
ssInt.eval(prog)
Out[18]:
[0.00011874852207310614,
 0.00011874852207310614,
 0.00011874852207310614,
 0.00015723991637302734,
 0.00015723991637302772,
 0.00015723991637302755,
 0.00015723991637302712]
In [19]:
screenshot()

Something similar can be also done in the opposite way ...

In [20]:
expr = "SELECT obj, volume FROM kitchen WHERE below(table, this) AS dict;"
prog = ssPar.compile(expr)
ssInt.eval(prog)
Out[20]:
[{u'obj': RaveGetEnvironment(1).GetKinBody('chair_6b56'),
  u'volume': 0.030726317000293702},
 {u'obj': RaveGetEnvironment(1).GetKinBody('chair_083c'),
  u'volume': 0.025596275454522633}]

Or to check which objects are within another object ...

In [21]:
expr = "microwave = SELECT obj FROM kitchen WHERE id(this) == 'microwave' AS value; \
SELECT obj, position FROM kitchen WHERE within(microwave, this) AS list;"
prog = ssPar.compile(expr)
print ssInt.eval(prog)
[RaveGetEnvironment(1).GetKinBody('plate_b4e7'), [-1.1299999999999999, 2.0099999999999998, 1.1399999999999999]]

In [22]:
screenshot()

7. Complex Queries

With the following Python command we simply switch on the lights or in other words, switch on the sensory systems and start to render the sensory measurements.

In [23]:
for sensor in env.GetSensors():
    sensor.Configure(Sensor.ConfigureCommand.PowerOn)
    sensor.Configure(Sensor.ConfigureCommand.RenderDataOn)
In [24]:
screenshot()

To check which of those sensors are currently measuring a robot, we can apply the following select query. The function to in this example is only used to translate the function name and thus the key of the dictionary from id to sensor and robot. This query simply evaluates the cross product of kitchen and kitchen, which is translated into e1 and e2.

Another remarkable aspect in this query is the new keyword this which is applied to place objects into the appropriate place, if functions are called with more than one parameter. So far there was no need for a this pointer, because all called functions required only one parameter, which is automatically substituted with this. For example the function-call volume after a select statement can also be written as volume(this). And if we use more than one "list" have to apply the appropriate name in front of the this pointer, such as e1.this and e2.this.

In [25]:
expr = "SELECT to(id(e1.this), 'sensor'), to(id(e2.this), 'robot')                   \
        FROM e1=kitchen, e2=kitchen                                                  \
        WHERE isSensor(e1.this) AND isRobot(e2.this) AND isSensing(e1.this, e2.this) \
        AS dict;"
prog = ssPar.compile(expr)
ssInt.eval(prog)
Out[25]:
[{u'robot': u'roomba_625x', u'sensor': u'barrett_4918_laser_scaner'}]

The same query can also be simplified with multiple queries, which in the first case queries only for sensor systems and stores the results within a list structure. The same is done for robots afterwards, before these results are combined within the last query.

In [26]:
expr = "sensor = SELECT obj FROM kitchen WHERE isSensor(this) AS list;     \
        robot  = SELECT obj FROM kitchen WHERE isRobot(this)  AS list;     \
                                                                           \
        SELECT to(id(sensor.this), 'sensor'), to(id(robot.this), 'robot')  \
        FROM sensor, robot                                                 \
        WHERE isSensing(sensor.this, robot.this)                           \
        AS dict;"
prog = ssPar.compile(expr)
ssInt.eval(prog)
Out[26]:
[{u'robot': u'roomba_625x', u'sensor': u'barrett_4918_laser_scaner'}]

Or as another possibility, we can simply place queries wherever we want ...

In [27]:
expr = "SELECT to(id(e1.this), 'sensor'), to(id(e2.this), 'robot')                   \
        FROM e1=(SELECT obj FROM kitchen WHERE isSensor(this) AS list),              \
             e2=(SELECT obj FROM kitchen WHERE isRobot(this) AS list)                \
        WHERE isSensor(e1.this) AND isRobot(e2.this) AND isSensing(e1.this, e2.this) \
        AS dict;"
prog = ssPar.compile(expr)
ssInt.eval(prog)
Out[27]:
[{u'robot': u'roomba_625x', u'sensor': u'barrett_4918_laser_scaner'}]

8. Interaction between Languages

The following examples is used to demonstrate how both languages Python a SelectScript can be appropriately combined. At first we might be interested in defining a new relation (a function) to the interpreter that checks whether an object is placed on another. The following function can be used to check this, whereby the programmer has to take care about the data types. o1 and o2 have to be objects of type kinbody or robot, otherwise the call of this function will cause an error. Therefore, be aware to use on only with the appropriate data types or insert some additional checks ...

In [28]:
def on(o1, o2):
    if o1 == o2: return False
    
    ab  = o1.ComputeAABB()
    pos = o2.GetTransform()[:3,3]
    
    if ab.pos()[0] - ab.extents()[0] <= pos[0] <= ab.pos()[0] + ab.extents()[0]:
        if ab.pos()[1] - ab.extents()[1] <= pos[1] <= ab.pos()[1] + ab.extents()[1]:
            if pos[2] >= ab.pos()[2] + ab.extents()[2]:
                return True
    
    return False

Attaching this function to the SelectScript interpreter is rather simple. Use the following method and include some additional information about the appropriate usage.

In [29]:
ssInt.addFunction('onX',   # a name for the function 
                  on,      # the function pointer
                  # some helpfull information
                  """Checks if the position of an object (o2) is above object (o1).
                     Usage: onX(o1, o2) -> bool """)

Afterwards it should also be possible to get some help about the newly defined function.

In [30]:
expr = "help('onX');"
prog = ssPar.compile(expr)
print ssInt.eval(prog)
onX
Checks if the position of an object (o2) is above object (o1).
                     Usage: onX(o1, o2) -> bool 

With the next query, we can simply identify all relevant parameters, which are required to fulfill the job of cleaning up the table.

In [31]:
robot = env.GetRobot("barrett_4918")
ssInt.addVariable('robot', robot)
expr  = "sink    = SELECT obj FROM kitchen WHERE id(this) == 'sink' AS value;"
expr += "table   = SELECT obj FROM kitchen WHERE id(this) == 'table' as value;"    
expr += "cleanUp = SELECT obj FROM kitchen WHERE onX(table, this) AND distance(robot, this) < 1.3 AS list;"
    
prog = ssPar.compile(expr)
ssInt.eval(prog)
Out[31]:
[RaveGetEnvironment(1).GetKinBody('cup_905a'),
 RaveGetEnvironment(1).GetKinBody('cup_58a2'),
 RaveGetEnvironment(1).GetKinBody('cup_b6af'),
 RaveGetEnvironment(1).GetKinBody('cup_d441')]

The example below shows how the results of a query can be easily applied within the program to solve the task of cleaning up the table...

In [32]:
from openravepy.examples.graspplanning import * # import the possibility to grasp
grasp = GraspPlanning(robot)
grasp.graspables = []
for object in ssInt.callVariable('cleanUp'):
    destination = grasp.setRandomDestinations( [object], 
                                                ssInt.callVariable('sink'), 
                                                randomize=True, preserverotation=False)
        
    gModel = databases.grasping.GraspingModel(robot=robot, target=object) 
    if not gModel.load():
        gModel.autogenerate()
    destination = [x.tolist() for x in destination[0]]        
    grasp.graspables.append([gModel,destination])

grasp.performGraspPlanning(withreplacement=True)
robot resolutions:  [ 0.00539782  0.00540015  0.01514345  0.01333564  0.06625722  0.06928872
  0.11318399  0.0446961   0.0446961   0.0446961   0.0302158 ]
robot weights:  [ 1.80578883  1.37230776  0.563284    0.34101693  0.13790135  0.1103156
  0.05769339  0.03253983  0.03253983  0.03253983  0.13534079]
searching for graspable objects (robot=3b8cffafe260780413124da7341b08b5)...
cup_905a is graspable
cup_58a2 is graspable
cup_b6af is graspable
cup_d441 is graspable
searching for destinations on table...
searching for destinations on sink...
searching for destinations on sink...
searching for destinations on sink...
searching for destinations on sink...
starting to pick and place random objects
grasping object cup_b6af
grasp 120 initial planning time: 1.730601
moving hand
current robot array([ -1.13996407e+00,   1.13589385e+00,   1.15916982e-16,
         1.06005976e+00,  -3.14159265e+00,  -9.45639038e-01,
        -2.71076040e+00,   0.00000000e+00,   0.00000000e+00,
         0.00000000e+00,   2.22044605e-16])
global direction array([ -2.88468562e-15,  -1.20880337e-15,  -1.00000000e+00]) [ 0.  0. -1.]
local direction [ 0.  0. -1.]
move hand up
planning to destination
failed to reach a goal, trying to move goal a little up openrave planning_error: 'MoveToHandPosition'
moving hand down
success:  120
grasping object cup_905a
grasp 284 initial planning time: 8.731741
moving hand
current robot array([ -7.66864614e-01,   1.73305242e+00,   2.22387527e+00,
        -4.06126613e-01,  -2.26437844e+00,  -5.00889999e-01,
         1.17509283e+00,   0.00000000e+00,   0.00000000e+00,
         0.00000000e+00,   2.22044605e-16])
global direction array([ 0.49713642, -0.8617768 ,  0.10097587]) [ 0.49713642 -0.8617768   0.10097587]
local direction [ 0.49713642 -0.8617768   0.10097587]
move hand up
planning to destination
moving hand down
success:  284
grasping object cup_58a2
grasp 454 initial planning time: 22.531033
moving hand
current robot array([  1.46221570e+00,  -1.59727151e+00,   1.68750079e-01,
        -8.10719390e-01,  -1.29556888e+00,   1.26036674e+00,
         2.72798152e+00,  -1.24900090e-16,   0.00000000e+00,
         0.00000000e+00,   1.13242749e-14])
global direction array([ 0.86178901, -0.49714411,  0.1008337 ]) [ 0.86178901 -0.49714411  0.1008337 ]
local direction [ 0.86178901 -0.49714411  0.1008337 ]
move hand up
planning to destination
moving hand down
success:  454
grasping object cup_d441
grasp 461 initial planning time: 9.782652
moving hand
current robot array([ 2.01726634, -1.68979145, -0.87473952, -0.58246952, -3.80170288,
       -1.56600089, -0.46809433,  0.        ,  0.        ,  0.        ,  0.        ])
global direction array([  9.94888337e-01,  -4.83856759e-07,  -1.00981171e-01]) [  9.94888337e-01  -4.83856760e-07  -1.00981171e-01]
local direction [  9.94888337e-01  -4.83856760e-07  -1.00981171e-01]
move hand up
planning to destination
moving hand down
robot in collision, moving back a little
success:  461
grasping object cup_905a
failed to grasp object cup_905a

The video below shows the whole cleanup run ...

In [33]:
YouTubeVideo('jSaoCXRNVNg', width=640, height=430)
Out[33]:

9. Callbacks

In some cases it might be useful to execute a specific function if a situation has changed, or in other words, if the result of a SelectScript has changed. The callback from below simply receives the result of a SelectScript query and stores the results within a global variable.

In [34]:
cbResult = []
def callback(result):
    global cbResult
    cbResult = result

The script below was taken from the examples in section "Complex Queries". The only difference is, that we do not execute the program directly but add trigger, which is fired if the result of the query changes and thus executes function callback, whereby the input parameters are always the result of a query.

In [35]:
expr = "env  = kitchen;                                                              \
        SELECT to(id(e1.this), 'sensor'), to(id(e2.this), 'robot')                   \
        FROM e1 = env, e2 = env                                                      \
        WHERE isSensor(e1.this) AND isRobot(e2.this) AND isSensing(e1.this, e2.this) \
        AS dict;                                                                     "
prog = ssPar.compile(expr)
ssInt.addTrigger('sensing_sensors', prog, 1, callback)

The both examples below simply show the changing results of triggered query...

In [36]:
print cbResult
[{u'sensor': u'barrett_4918_laser_scaner', u'robot': u'roomba_625x'}]