Community:Science/Robotics/Middleware/YARP

YARP + Blender

There are examples for using YARP with Blender here:

• Source code: https://code.launchpad.net/~blender-for-robotics/openrobots-simulator/trunk
• See subdirectory: tutorials/yarp/ for very small self-contained demos. There are larger demos elsewhere in the repository.

A "robot" doing edge-following using a radar sensor and the left-hand rule; uses an external controller connected to Blender via YARP.

Installation

• Grab YARP (> 2.2.3) source from YARP website: [1]
• Follow installation instructions (basically, ./configure, make, make install)
• Compile the YARP bindings: see here [2], and create a directory where to store your pythonic libraries for robotics:

From the yarp2.2.x/example/swig directory,
$ mkdir [path of your choice, like ~/blender_robotics/lib]
$ cp yarp.py [your path]
$ cp _yarp.so [your path]

I recommend to add a environment variable (like $BLENDER_ROBOTICS_ROOT) in you ~/.login that points to this path.

If you do so, you can include the lib in your Blender scripts with this Python snippet:

import sys, os

try:
   libRoot = os.path.join(os.environ['BLENDER_ROBOTICS_ROOT'],'lib')
except KeyError:
   libRoot = '.'

sys.path.append(libRoot)

You can also install it system-wide:

$ sudo cp yarp.py /usr/lib/python2.5/site-package/
$ sudo cp _yarp.so /usr/lib/python2.5/site-package/_yarp.so

Some of the YARP+Blender tutorials assume a system-wide installation. If you don't do this, please add the Python snippet above as appropriate when trying out tutorials.

Image Stream Demo

In directory tutorials/yarp/simpleImageCapture/, there is an example of streaming images from blender via a yarp port. This is handy to do vision processing outside of Blender.

When working with YARP, it is a good ideal to launch Blender from a console to keep Python outputs visible. YARP will print messages on the console.

In Blender's text editor, make a script called "init_yarp" with the following content:

import yarp
yarpNetwork = yarp.Network()

In the script panel, turn on "Enable Script Links" and add a new "OnLoad" link to the "init_yarp" script. This ensures that YARP gets set up outside the game engine (there seem to be python stability problems if it is set up within the game engine). Save your work so far, and reopen it. If you get any errors, make sure you have a yarp server running.

Now add a script called "send_image" with the following content:

import Blender
import Rasterizer
from Blender.BGL import *
import array
import yarp
import GameLogic

Rasterizer.showMouse(1)
coords = Blender.Window.GetMouseCoords()

try:
	p = GameLogic.globalDict['yarpCameraPort']
except:
	# Open a "port" for sending images.
	# Let's call it "/blender/cam".
	p = yarp.Port()
	p.open("/blender/cam")
	GameLogic.globalDict['yarpCameraPort'] = p

# For testing, let's try to connect to a port called /img/read if available.
if p.getOutputCount()==0:
	yarp.Network.connect("/blender/cam","/img/read")

# Get data from OpenGL
imX = 256
imY = 256
buf = Buffer(GL_BYTE,[imX*imY*3])
glReadPixels(coords[0]-imX/2,coords[1]-imY/2,imX,imY,GL_RGB,GL_UNSIGNED_BYTE,buf)

# Convert it to a form where we have access to a memory pointer
data = array.array('B',buf.list)
info = data.buffer_info()

# Wrap the data in a YARP image
img = yarp.ImageRgb()
img.setTopIsLowIndex(0)
img.setExternal(info[0],imX,imY)
# yarpview doesn't currently respect orientation flag, so make a clean copy
img2 = yarp.ImageRgb()
img2.copy(img)
# Write the YARP image
p.write(img2)
print "Wrote an image"

In the Logic panel, hook up an "always" sensor to run this script periodically.

To test your program, make sure that yarp server and yarpview /img/read are running in two other terminals. In Blender, enter the game engine mode P and you should see in yarpview an image stream corresponding to what is under your mouse.

Here's a yarp viewer overlaid on a blender application in game mode:

Blender in one window, with an external image stream viewer overlaid.

Command Port Demo

In directory tutorials/yarp/simpleTranslation/, Boris Duran has an example of moving a cube around via a yarp port, with gravity turned off. There's also a demo in tutorials/yarp/jumpMonkeyJump/ with gravity turned on, where you can command a monkey to jump in the air and then watch it fall again.

These demos require the same init_yarp script used in the previous demo:

import yarp
yarpNetwork = yarp.Network()

The "jumpMonkeyJump" demo moves Suzanne around based on input from a port, with the following script attached to an "always" sensor:

import Blender
import GameLogic
import yarp

try:
	p = GameLogic.globalDict['controlPort']
except:
	GameLogic.globalDict['controlPort'] = yarp.BufferedPortBottle()
	p = GameLogic.globalDict['controlPort']
	p.open("/blender/move")

bot = p.read(False)
if bot!=None:
  	scene = GameLogic.getCurrentScene()
	xx = bot.get(0).asDouble()
	yy = bot.get(1).asDouble()
	zz = bot.get(2).asDouble()
	obj = scene.objects["OBSuzanne"]
	obj.setLinearVelocity([xx,yy,zz])

The try/except clause makes a port called "/blender/move" if one hasn't already been made. The script then polls that port for input with p.read(False) (the False means "don't wait", True would block and wait for input to arrive). If input is received, it is treated as a vector of three floating point numbers, and used to set the linear velocity of Suzanne.

To send commands to this port from the command line, you can do something like:

 yarp write /test /blender/move

and then type three numbers, like:

 10.0 10.0 10.0

Of course you could do this from a program too, in C++ or any of the languages that YARP has wrappers for (Python, Perl, Java, Matlab-via-Java, Tcl, Ruby, Lisp, ...)

Wall following demo

In directory tutorials/yarp/wallFollow/', there is an example of commanding forward and turning motions, and using a very basic proximity sensor. There is an example of implementing a wall following behavior directly within the game engine, or in a separate program via YARP.

Again, this demo has the same "init_yarp" script for starting up YARP. And then it has a script that runs "always" with some frequency, that now creates two ports, one for receiving commands, and one for sending the state of the radar sensor:

import Blender
import GameLogic
import yarp

# control port - input
try:
	p = GameLogic.globalDict['controlPort']
except:
	GameLogic.globalDict['controlPort'] = yarp.BufferedPortBottle()
	p = GameLogic.globalDict['controlPort']
	p.open("/blender/speed")

# radar port - output
try:
	r = GameLogic.globalDict['radarPort']
except:
	GameLogic.globalDict['radarPort'] = yarp.BufferedPortBottle()
	r = GameLogic.globalDict['radarPort']
	r.open("/blender/radar")

bot = p.read(False)
if bot!=None:
  	scene = GameLogic.getCurrentScene()
	forwardSpeed = bot.get(0).asDouble()
	turnSpeed = bot.get(1).asDouble()
	obj = scene.objects["OBSuzanne"]
	obj.setLinearVelocity([0,0,forwardSpeed],True)
	obj.setAngularVelocity([0,turnSpeed,0],True)

# output the current state
try:
	radar = GameLogic.globalDict['radar']
	state = r.prepare()
	state.clear()
	state.addInt(radar)
	r.write()
except:
	print "No radar"

Our input port is "/blender/speed", and we expect a pair of floats to control forward speed and turning speed respectively. Our output port is "/blender/radar", and it carries a single integer (just an on/off flag for the radar). The state of the radar is set in scripts triggered when a radar sensor goes on or off.

When the Game Engine is running, an "AI" program that is entirely external to Blender could be controlling the robot. Here is that "AI" written in python (a very crude implementation of the left-hand/right-hand rule for getting through simple mazes, just follow the wall on your left/right):

#!/usr/bin/python
import yarp
net = yarp.Network()

radarInput = yarp.BufferedPortBottle()
radarInput.open("/monkey/client/radar")
net.connect("/blender/radar","/monkey/client/radar")

motorOutput = yarp.BufferedPortBottle()
motorOutput.open("/monkey/client/speed")
net.connect("/monkey/client/speed","/blender/speed")

while True:
    radar = radarInput.read(False)
    if radar!=None:
        contact = radar.get(0).asInt()
        print "Got radar", contact
        forwardSpeed = 1.5
        turnSpeed = 0.5
        if contact:
            turnSpeed *= -1
        command = motorOutput.prepare()
        command.clear()
        command.addDouble(forwardSpeed)
        command.addDouble(turnSpeed)
        print "Command:", command.toString()
        motorOutput.write()
    yarp.Time.delay(0.1)

Of course this is a trivial controller, and would be better off written within the Game Engine itself. But for robot controllers involving computer vision or extensive planning, working outside Blender could be very convenient.

Big Demo

For a more complete example, see Robotics:Yarp Python Simulator Example