From BlenderWiki

Jump to: navigation, search

[edit] Setup of a simple simulation with Python and YARP

[edit] Overview

This page explain how to set up a simple simulation: a robot with a camera evolves in an simple environment (controlled by the keyboard). The camera of the robot is streamed with YARP to another application (dedicated to image processing for instance. In our case, we use the yarpview tool to simply display the camera stream), and the robot detect all the objects it sees and output the name, type and color of the object to another YARP port.

During the simulation, the user can click on an object in the environment to move it.

A short screencast of the result is available at http://www.youtube.com/watch?v=OT1Ck_RE5Wg


[edit] Before starting...

  • This tutorial has been written with Blender 2.48.1 (April 2009)
  • A blend file will the complete set up is available at the end of this page.
  • Please be indulgent with my Python coding style: this is my first face-to-face experience with this language. Improvements to the scripts are more than welcome!

[edit] The simulator

These are the main steps:

  • Scene creation + keyboard control of the robot
  • Initialization & finalization of the simulation
  • Objects selection and displacement
  • Object detection and output with YARP
  • Image grabbing and output with YARP

Here a screenshot of my Blender workspace. I will often refer to it, so I could help to keep it open in a separate tab of your browser.

[edit] Scene creation + keyboard control of the robot

I won't go much into details for this part. A lot a good tutorial are available on the web, for instance here:

Some specific hints:

  • We use 2 cameras, one for the robot, one for the user. Call them (press N to access the dialog where you can modify the name) CameraRobot and CameraUser. Parent the CameraRobot to your robot. Thus it will move with the robot.
  • You can enable physics as well.
  • Add an Empty and call it "ScriptHolder". We will store most of our simulation logic in this object.

You should have now a working scene: if you start the Game Engine (key P), you can move the robot around. In the next step, will set up initialization and finalization script. It is needed to set up the Game Engine viewport, for instance.

[edit] Initialization & finalization of the simulation

  • Create two new empty scripts, initialization.py and finalization.py
  • Add logic bricks to the ScriptHolder object to trigger the initialization and finalization script at the beginning and at the end. Cf below:
  • Below is the initialization script. Comments explain what happen. 

import Rasterizer, Blender, GameLogic

print '######## SIMULATOR INITIALIZATION ########'
print

#Display mouse cursor
Rasterizer.showMouse(1)

# get game window height and width
height = Rasterizer.getWindowHeight()
width = Rasterizer.getWindowWidth()

# get the active camera
scene = GameLogic.getCurrentScene()

# get the active controller
cont = GameLogic.getCurrentController()

# get cameras from object list
camUser = scene.getObjectList()["OBCameraUser"]
camRobot = scene.getObjectList()["OBCameraRobot"]

#get a reference to a scene actuator, needed to change the active camera
sceneCamera = cont.getActuator('sceneCamera')


######## Set viewports ##########

# camUser viewport: entire screen
left_A = 0
bottom_A = 0
right_A = width
top_A = height

camUser.setViewport( left_A, bottom_A, right_A, top_A)

#  camRobot viewport: corner top right
left_B = width * 2 / 3
bottom_B = height * 2 / 3
right_B = width
top_B = height
           
camRobot.setViewport( left_B, bottom_B, right_B, top_B) 
camRobot.setOnTop()

#Stores the position of the robot's camera in the OpenGL window for later image 
#grabbing ([bottom left corner X,bottom left corner Y, width, height])
GameLogic.camRobotViewport = [left_B, bottom_B, right_B - left_B , top_B - bottom_B]

#Important: activate the viewport (else, nothing happen!)
camUser.enableViewport(True)
camRobot.enableViewport(True)

# Set the active camera
sceneCamera.setCamera(camUser)
GameLogic.addActiveActuator(sceneCamera, True)

########## Create and store global variables ###########

#If they were not yet created, we initialize them as member of the global GameLogic object.

#trackedObject is a dictionary containing the list of tracked objects (->meshes with
# a class property set up) as keys and the bounding boxes of these objects as value.
if not hasattr(GameLogic, 'trackedObjects'):
	print ' * Initialization of trackedObjects variable...'
	GameLogic.trackedObjects = dict.fromkeys([ obj for obj in scene.getObjectList() if 'objClass' in obj.getPropertyNames() ])
	for obj in GameLogic.trackedObjects:
		GameLogic.trackedObjects[obj] = Blender.Object.Get(obj.name[2:]).getBoundBox(0) #getBoundBox(0) returns the bounding box in local space instead of world space.
		print '  - ',obj.name

#visibleObjects stores the list of currently visible objects by the robot.
if not hasattr(GameLogic, 'visibleObjects'):
	print ' * Initialization of visibleObjects variable...'
	GameLogic.visibleObjects = []

#selectedObject stores the current selected object
if not hasattr(GameLogic, 'selectedObject'):
	print ' * Initialization of selectedObject variable...'
	GameLogic.selectedObject = None

#ugly hack to solve the double triggering in object_selection.py that occur 
# when an object is selected.
if not hasattr(GameLogic, 'ignoreFlag'):
	GameLogic.ignoreFlag = False
	
print
print '######## END OF INITIALIZATION ########'
print

And the finalization script (not very useful right now, but we will need it later to cleanly close some resources). 


import GameLogic

#retrieve a reference to the sensor which triggered this script
sensor = GameLogic.getCurrentController().getSensor('escape_key')

#execute only when the ESC key is released (if we don't test that, the code 
#get executed two time, when pressed, and when released)
if not sensor.isPositive() and sensor.isTriggered():
	quitActuator = GameLogic.getCurrentController().getActuator('quit')
	GameLogic.addActiveActuator(quitActuator, True)
	
	print '######### EXITING SIMULATION ########'
Add an emergency exit...:
I've added as well a sensor on the F12 key, directly linked to the "quit" actuator. It skips the finalization script, and quit the simulator. It useful when, for some reason (and there's plenty of reasons :-/), the finalization script encounters an error: when it happen, there's not way to leave the GameEngine without killing Blender...


[edit] Objects selection and displacement

We need two things: first, select an object when it is clicked, Then, move it.

  • To select objects, we must add some logic to every object we want to be selectable.

For instance, for the "YellowBox":

The content of the object_selection script: 


import GameLogic

cont = GameLogic.getCurrentController()

#return the object for which the script has been triggered
own = cont.getOwner()

mouseOver = cont.getSensor("mouseOver")
leftClick = cont.getSensor("leftClick")

#ugly hack to solve the double triggering that occur when an object is selected.
if GameLogic.ignoreFlag:
	GameLogic.ignoreFlag = False
else:
	GameLogic.ignoreFlag = True

	#Object is already selected
	if (GameLogic.selectedObject == own):
		#We release the object on click
		if leftClick.isTriggered():
			GameLogic.selectedObject = None

			#Re-insert the object in the physics simulation
			own.restoreDynamics()
			own.setLinearVelocity([0, 0, 0])
			own.setAngularVelocity([0, 0, 0])

			print own.getName(), 'deselected'
					
	#Object is not selected
	else:
		#We set the selectObject variable to this object
		if mouseOver.isPositive() & leftClick.isTriggered():
			GameLogic.selectedObject = own

			#Remove the object from physics simulation
			own.suspendDynamics()

			print own.getName(), 'selected'
  • Then, if an object is selected, we want to move it. First add a mouseOverAny sensor to the ScriptHolder object:

and link it to the object_pick script. 


import Rasterizer

#if no object is selected, return immediately.
if GameLogic.selectedObject != None:

	# get controller
	cont = GameLogic.getCurrentController()

	target = GameLogic.selectedObject #as defined in object_selection script
	target_pos = target.getPosition()

	mouseOverAny = cont.getSensor("mouseOverAny")

	if mouseOverAny.isPositive():
		hit_pos = mouseOverAny.getHitPosition() #return the 3D point which is "touched" by the mouse

		#change the X and Z position of the selected object. Y is kept. So this script is
		# "usable" only if the CameraUser looks roughly at the (X,Z) plan.
		target.setPosition([hit_pos[0], target_pos[1], hit_pos[2]])

As stated in the comments, this script does a very basic job, and will correctly work only if the CameraUser looks at the (X,Z) plan. It could be improved by moving the selected object in the plan orthogonal to the camera.

You can of course test everything at any time by pushing the magic P key.

[edit] Object detection and output with YARP

Ok. We are now entering the "robotic" part of the tutorial.

The first thing we will set up is what I call a "cognitive map simulator": I want the robot to output the list of the object it sees, with their position, color and type (bottle, table, etc.). I need to retrieve it outside of Blender (actually, I'm filling an ontology with these data), so we'll rely on a middleware (YARP in this case) to transport the data where we need them (another application, another computer, as you wish...)

  • Add a string property named "objClass" to objects you want to detect, and set the value to the class of the object you want (for instance "table", "bottle", etc. It does not really matter). This "objClass" property is just an example. You could use any name and any value, depending on your need. Go back to the initialization script, and check that the code that initialize the trackedObject variable select the objects you want.
  • Add a new "Always" sensor the ScriptHolder with these settings:

It will call every 20 "logic ticks" the check_visibility script.

Here is the script, a bit more complex than the previous ones: 


import Blender, GameLogic
from Blender import Material, Object

scene = GameLogic.getCurrentScene()

# get OBCameraRobot from object list.
#We'll use it to check if tracked objects are visible or not.
cam = scene.getObjectList()["OBCameraRobot"]

# GameLogic.trackedObjects is initialized in initialization.py. 
# It holds the list of tracked objects (basically, meshes) + their bounding boxes).
trackedObjects = GameLogic.trackedObjects
visibleObjects = GameLogic.visibleObjects

#Little helper fonction to calculate the hue of a color
def RGBtoHue(rgbList):
	R = rgbList[0]
	G = rgbList[1]
	B = rgbList[2]
	
	# min, max, delta;
	min_rgb = min( R, G, B )
	max_rgb = max( R, G, B )
	delta = max_rgb - min_rgb

	if not delta:
		return 0
	if R == max_rgb:
		H = ( G - B ) / delta # between yellow & magenta
	elif G == max_rgb:
		H = 2 + ( B - R ) / delta # between cyan & yellow
	else:
		H = 4 + ( R - G ) / delta # between magenta & cyan
	H *= 60 # convert to deg
	if H < 0:
		H += 360
	return int(H)

#this method retrieve the first material of the first mesh of an object
#and return the hue of this material.
def retrieveHue(obj):
	mesh = obj.getMesh(0) # There can be more than one mesh...
	if mesh != None:
		bMat = Material.Get(mesh.getMaterialName(0)[2:])
		return RGBtoHue(bMat.getRGBCol())
	
	return None

#the main method: check is an object lies inside of the camera frustrum. 	
def checkVisible(obj):
	# camera inside box?
	bb = trackedObjects[obj] #trackedObjects was filled at initialization with the object's bounding boxes
	pos = obj.getPosition()
	#translate the bounding box to the current object position and check if it is in the frustrum
	if cam.boxInsideFrustum([[bb_corner[i] + pos[i] for i in range(3)] for bb_corner in bb]) != cam.OUTSIDE:
	# object is inside
		return True

for obj in trackedObjects:
	#if the object is visible and no yet in the visibleObjects list...
 	if checkVisible(obj) and visibleObjects.count(obj) == 0:
		visibleObjects.append(obj)
		print obj.getName(), '(', obj.objClass,', hue:',retrieveHue(obj),') just appeared.'


	#if the object is not visible but was in the visibleObjects list...
 	if not checkVisible(obj) and visibleObjects.count(obj) != 0:
		visibleObjects.remove(obj)
		print obj.getName(), ' just disappeared.'
  • Check the simulation behaves as expected...
  • Then we can introduce YARP. Yeaaah! If you're not yet familiar with YARP and Blender, check this page first: Robotics:Middleware/YARP
Save often!:
Starting fiddling with YARP means a huge increase in the probability that Blender hang or crash. Save often!


I assume now that YARP (including the GUI yarpview) and its Python bindings are installed and work.

To organize things in a "not-too-dirty-way", we'll create a special package to store some very simple "YARP" methods (if there's some Python masters around here, improvements are welcome!).

Add an environment variable to your .bashrc or .tcshrc (or whatever shell resource script you're using) called BLENDER_ROBOTICS_ROOT that points to the script root.

For bash, something like: 


export BLENDER_ROBOTICS_ROOT=$HOME/blender_robotics/scripts

For tcsh, something like: 


setenv BLENDER_ROBOTICS_ROOT $HOME/blender_robotics/scripts

From your script directory, create this tree: 


$ mkdir middleware 
$ cd middleware ; echo > __init__.py
$ mkdir yarp
$ cd yarp ; echo > __init__.py

If you don't want to install YARP libs system-wide, you can put them in this newly created yarp directory.

Create in this same yarp directory a YarpBlender.py script, and paste this: 


import sys, os

try:
   scriptRoot = os.path.join(os.environ['BLENDER_ROBOTICS_ROOT'],'scripts')
except:
   scriptRoot = '.'

sys.path.append(scriptRoot)

import GameLogic, yarp

#The initialize() method takes 2 lists of strings.
#The first list holds port names which will be create as YARP Port,
#the second list holds port names which will be create as YARP BufferedPortBottle
#(cf YARP doc for the difference)
def initialize(portList, bufferedPortList):

	#Initialize the network and connect to the yarp server
	yarp.Network.init()
	
	#yarpPorts is a dictionary that hold a reference to the different YARP port used by the simulation.
	if not hasattr(GameLogic, 'yarpPorts'):
		print ' * Initialization of YARP and Yarp ports...'
		GameLogic.yarpPorts = dict.fromkeys(portList + bufferedPortList)
		#first create simple ports
		for portName in portList:
			print '  - Adding ', portName, ' port.'
			port = yarp.Port()
			port.open(portName)
			GameLogic.yarpPorts[portName] = port
		#then create BufferedPortBottle
		for portName in bufferedPortList:
			print '  - Adding ', portName, ' buffered port.'
			port = yarp.BufferedPortBottle()
			port.open(portName)
			GameLogic.yarpPorts[portName] = port

#Closes the ports and release the network
def finalize():
	for port in GameLogic.yarpPorts.itervalues():
		port.close()
		
	yarp.Network.fini()
	print ' * YARP ports have been closed.'
	
def getPort(portName):
	return GameLogic.yarpPorts[portName]

...This code can easily be improved. Paul, maybe?

As you can see, YARP requires some initialization and finalization. Let's add at the beginning of the initialization script: 


import sys, os

try:
   scriptRoot = os.path.join(os.environ['BLENDER_ROBOTICS_ROOT'],'scripts')
except:
   scriptRoot = '.'

sys.path.append(scriptRoot)

from middleware.yarp import YarpBlender

#YARP initialization
YarpBlender.initialize(['/blender_simu/cam'], ['/blender_simu/cog_map'])

(we will use the /blender_simu/cam port later)

And the finalization script becomes: 


import GameLogic
from middleware.yarp import YarpBlender

sensor = GameLogic.getCurrentController().getSensor('escape_key')

#execute only when the ESC key is released (if we don't test that, 
#the code get executed two time, when pressed, and when released)
if not sensor.isPositive() and sensor.isTriggered():
	print '######### FINALIZING... ########'
	
	YarpBlender.finalize()
	quitActuator = GameLogic.getCurrentController().getActuator('quit')
	GameLogic.addActiveActuator(quitActuator, True)
	
	print '######### EXITING SIMULATION ########'
  • Then, in check_visibility, you can add this kind of code when an object appear (or disappear): 

		# prepare the YARP message...
		bottle = yarp_port.prepare()
		bottle.clear()
		bottle.addString(obj.getName() + '(' + obj.objClass + ') just appeared.')
		bottle.addInt(retrieveHue(obj))
		#...and send it
		yarp_port.write()

To test that:

  • Start yarp server
  • Launch yarp read /reader
  • Launch the GameEngine (key P)
  • Connect them with yarp connect /blender_simu/cog_map /reader

Now, when objects enter in your robot camera field, you should get the info in the yarp read terminal.

Easy, isn't it? (well maybe right now, nothing at all works for you, and you are trying to throw your computer through the window... don't do that, it's not eco-friendly at all!)

[edit] Image grabbing and output with YARP

The last by not the least part of the tutorial: we will now had video streaming to our application.

Save very often!:
Playing with YARP + the OpenGL buffer to grab the camera image means that Blender will crash often. Save very often! I mean, always hit Ctrl W before P!


  • Add a keyboard sensor called "grab_image" to ScriptHolder, and set it to fire a Python controller linked to a new "image_grabber" script.
  • The script looks like that: 

import Blender, yarp

from Blender.BGL import *

from middleware.yarp import YarpBlender

import array

sensor = GameLogic.getCurrentController().getSensor('grab_image')

ok = sensor.isPositive() and sensor.isTriggered()

#execute only when the 'grab_image' key is released (if we don't test that, the code get executed two time, when pressed, and when released)
if ok:
	#retrieve the YARP port we want to write on	
	p = YarpBlender.getPort('/blender_simu/cam')
	
	#retrieve the size and position of the robot camera, as stored in the initialization script
	v = GameLogic.camRobotViewport
		
	imX = v[2]
	imY = v[3]
	buf = Buffer(GL_BYTE,[imX*imY*3])
	
	#Call to the OpenGL library to copy the framebuffer to the main memory
	#The value 370 and 340 depend of the configuration of your script. I couldn't
	#figure out a way to get with Pythonthe absolute position of the GameEngine
	#window.
	glReadPixels(370 + v[0], 340 + v[1],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.setQuantum(1)
	img.setExternal(info[0],imX,imY)
	
	# copy to image with "regular" YARP pixel order
	img2 = yarp.ImageRgb()
	img2.copy(img)
	
	# Write the image
	p.write(img2) 
	
	print "Wrote an image"

To test that:

  • Start yarp server
  • Launch yarpview /img
  • Launch the GameEngine (key P)
  • Connect them with yarp connect /blender_simu/cam /img
  • In the GameEngine, fire the image capture by pressing the key you set for your sensor.


As howework, I let you add a sensor that call often enough (actually on my machine, Blender was perfectly responsive even with an "Always" sensor with f=2. However f=5 should probably be quick enough) the "image_grabber" script, to actually stream the camera to YARP.

Here we are!

A last screenshot of the ScriptHolder object with all the sensors/controller/actuators:

[edit] Resources

Retrieved from "http://wiki.blender.org/index.php/Robotics:Middleware/YARP/Python_Simulator_Example"