Interesting Survey about different Robotic Software Frameworks for Distributed Robotics

This work identifies many aspects of the distributed robotics and compare ROS and other Robotic Frameworks for distributed robotics. Here I show the abstract:

Abstract

Robotics is an area of research in which the paradigm of Multi-Agent Systems (MAS) can prove to be highly useful. Multi-Agent Systems come in the form of cooperative robots in a team, sensor networks based on mobile robots, and robots in Intelligent Environments, to name but a few. However, the development of Multi-Agent Robotic Systems (MARS) still presents major challenges. Over the past decade, a high number of Robotics Software Frameworks (RSFs) have appeared which propose some solutions to the most recurrent problems in robotics. Some of these frameworks, such as ROS, YARP, OROCOS, ORCA, Open-RTM, and Open-RDK, possess certain characteristics and provide the basic infrastructure necessary for the development of MARS. The contribution of this work is the identification of such characteristics as well as the analysis of these frameworks in comparison with the general-purpose Multi-Agent System Frameworks (MASFs), such as JADE and Mobile-C.

Keywords

  • Robotics;
  • MAS;
  • Agents;
  • Software frameworks;
  • Middleware;
  • Architecture

Cambiando el marco de referencia de una distribución Gaussiana

El trabajo de funciones probabilísticas gausianas en el espacio es algo muy habitual en robótica. Este tipo de distribuciones permite expresar una posición o pose (por ejemplo de un objeto u objetivo) de la cual se tiene cierta incertidumbre. Desde mi punto de vista se trata de una temática bastante interesante y me está permitiendo profundizar en algunos conceptos matemáticos como autovalores, números complejos y quaterniones. Otros posts previos que realicé en el pasado con temáticas relacionadas son:

Una función gausiana está compuesta de su vector media \mu y su matriz de covarianza \Sigma y se denota como P(x)\sim\mathcal{N}(\mu,\Sigma). El problema consiste en modificar estos parámetros para pasar del marco de referencia F_{A} al marco de referencia F_{B}. Estos marcos de referencia están descritos por una transformación lineal que incluye rotación y translación respecto a un marco de referencia universal, de modo que:

F_{A}\equiv(R_{A},T_{A})

F_{B}\equiv(R_{B},T_{B})

Cambio del marco de referencia del vector media \mu

El cambio de referencia del vector media es trivial dada una transformación TF entre marcos de referencia básicamente consiste en aplicar la transformación lineal al vector \mu_{A}:

\mu_{B} = R \mu_{A} + T

Donde:

R=R_{B}R_{A}^{-1}
T=T_{B}-R_{B}T_{A}

De echo en ocasiones los valores universales de los marcos de referencia R_{A}, T_{A}, R_{B}  y T_{B} no son conocidos, sino que directamente la relación entre ambos marcos TF\equiv (R,T).

Cambio del marco de referencia de la matriz de Covarianza \Sigma

¿Pero que pasa con la matriz de covarianza? En el caso de un problema unidimensional, el cambio de referencia del origen de coordenadas no afectaría a la matriz de covarianza \Sigma=\begin{bmatrix} \sigma^{2} \end{bmatrix} (que es la varianza). Por ejemplo: P(x)\sim\mathcal{N}(\mu_{A}=3,\Sigma_{A}=5) se convertiría en P(x)\sim\mathcal{N}(\mu_{B}=-7,\Sigma_{B}=5) si cambiaramos el origen de coordenadas desde el O=0 a O=10.

Para una función gausiana 2D o 3D es mucho más complicado. La matriz de covarianza representa la orientación de la distribución además de la  escala de la variabilidad en dentro de los ejes principales de esta rotación. A la derecha se muestra una figura de una función gausiana 2D. En una función gaussiana (creo, a ver si un matemático me lo confirma) que la probabilidad se extiende en N vectores ortogonales (autovectores) con diferentes escalas (autovalores) verificando que:

\Sigma = E (l I) E^{t}

Siendo l el vector de autovalores, el punto curioso es que la matriz E es una matriz de rotación cuyas columnas son los autovectores de \Sigma.  Es intuitivo ver que si el marco de referencia cambia, las coordenadas de estos autovectores cambiará, sin embargo la escala (autovalores) se mantendrá constante. Parece entonces sencillo.

Si estamos trabajando en el marco de referencia F_{A}F_{B} es conocido, entonces basta con cambiar el marco de referencia de los autovectores:

E_{B}=RE_{A}

\Sigma_{B}=RE_{A}(lI)(RE_{A})^{t}

o lo que es lo mismo si tenemos la información completa de F_{A}F_{B}:

\Sigma_{B}=R_{B}R_{A}^{-1}E_{A}(lI)(R_{B}R_{A}^{-1}E_{A})^{t}

Casos particulares que aún no se resolver

Sin embargo existen casos particulares y estos son los que a mi, personalmente me inquietan y aún no tengo claro como resolver:

  1. ¿Qué ocurre si la matriz de covarianza no es diagonalizable? Por ejemplo, se tiene total certeza de su posición en X, su posición en Y y algo de incertidumbre en Z. Siendo \Sigma_{A}=\begin{bmatrix}  0 & 0 & 0\\  0 & 0 & 0\\  0 & 0 & 2.3  \end{bmatrix} ¿Podría obtenerse la matriz E_{A} si solo tenemos un autovector?
  2. ¿Qué ocurre cuando trabajamos (en lugar de con un vector “posición” en 3D) con una “pose en el espacio” xyz+rpy 6D con una matriz de covarianza de 36 valores, donde se mezclan distintas unidades? Este tipo de representación gausiana es también muy utilizada en robótica, sin embargo, el abordaje del problema de cambio de sistema referencia de este tipo de distribuciones es algo que no he logrado localizar en ningún sitio. Parece un caso muy extraño dado que además los valores de rpy dependen en sí mismos del los ejes x,y,z del marco de referencia actual.
Ideas problema 1:
En el ejemplo tratado parece claro que el único autovector existente es v_0=\begin{vmatrix}\\ 0\\ 0\\ 1 \end{vmatrix} y su autovector \lambda_0=2.3 ya que verifica que \Sigma v_0 = \lambda v_0 . Intuyo que dada la precondición de ortogonalidad de los autovectores de la matriz E(¿inventado?) parece razonable que solo necesitamos un autovector para calcular E. De hecho  cualquier E que verifique que l=\begin{vmatrix}\\ \lambda_0\\ \lambda_1=0\\\lambda_2=0 \end{vmatrix} y sea ortogonal parece suficiente ¿sera correcto este razonamiento?Duda: Pienso que una matriz de covarianza no nula debería tener al menos un autovector real, pero de esto, no estoy seguro. Si fuera así, el problema estaría resuelto.
Ideas problema 2:
Creo que dado que el cambio de coordenadas en robótica solo afectaría a los ejes x, y y z el problema se simplificaría. El vector \mu para x, y y z cambiaría de la misma forma. Por el contrario los valores rpy en lugar de ser rotados serían trasladados. \mu_{B}= R* \mu_{A} + T donde:
R=\begin{bmatrix}  R_{xyz} & \O \\  \O & I\\\end{bmatrix}
y
T=\begin{bmatrix} t_x \\ t_y \\ t_z \\ getRoll(R) \\ getPitch(R) \\ getYaw(R) \\\end{bmatrix}
De esta forma las componentes de \Sigma_A definidas por covarianzas angulares, como \sigma_{row,pitch}=cov(row,pitch) permanecerían constantes tras la transformación TF, mientras que las componentes de la matríz como \sigma_{x,yaw}=cov(x,yaw) cambiarían su valor. Sin embargo, todo esto son por ahora solo intuiciones. ¿es correcto este razonamiento?¿Alguna ayuda?

Basic Rotations with Quaternions

The quaternions is a very interesting mathematical tools that make possible perform rotation operations without using rotation matrices. This method is efficent and secure (avoiding the gimbal lock effect) in dynamic situations. A very interesting explanation about the mathematical foundations and capacities of quaternions can be found here.

The ROS library TF provide several functions to handle quaternions in C++ and Python. There exists many opensource libraries which provides quaternion funciontalities like Eigen, Bullet to name a couple of them.

Rotating a point
ROS works with the right hand axis convention. This axis convention is specially adequate for mobile robots. Here the robot is always looking to the axis-x infinity.

import roslib
roslib.load_manifest('tf')
import tf
from tf.transformations import *

# we want to rotate the point using the x-axis ("roll")
rot=quaternion_from_euler(-numpy.pi/4,0,0)

# the object is located in the y=1. We use the format [x,y,z,w] and w is allways 0 for vectors
vec= [0,1,0,0]

#now we apply the mathematical operation res=q*v*q'. We use this function for multiplication but internally it is just a complex multiplication operation.
result=quaternion_multiply(quaternion_multiply(rot, vec),quaternion_conjugate(rot))

In [33]: result
Out[33]: array([ 0. , 0.70710678, -0.70710678, 0. ])

Concatenate Rotations
Here I show how to concatenate multiple rotations using quaternions:

q1=quaternion_from_euler(-numpy.pi/4,0,0)
q2=quaternion_from_euler(numpy.pi/4,0,0)
q3=quaternion_from_euler(-numpy.pi/4,0,0)
q4=quaternion_from_euler(numpy.pi/4,0,numpy.pi/4)

#the full transform is q4*q3*q2*q1
rot= quaternion_multiply(q4,quaternion_multiply(q3,quaternion_multiply(q2,q1)))
result=quaternion_multiply(quaternion_multiply(rot, vec),quaternion_conjugate(rot))

In [86]: result1
Out[86]: array([ 0. , 0.70710678, -0.70710678, 0. ])

In [90]: result2
Out[90]: array([ 0., 1., 0., 0.])

In [93]: result3
Out[93]: array([ 0. , 0.70710678, -0.70710678, 0. ])

In [96]: result
Out[96]: array([-0.70710678, 0.70710678, 0. , 0. ])

Starting with rosjava and image messages

This post is a mini-tutorial based on the basic rosjava tutorials whose code can be found in [https://rosjava.googlecode.com/hg/) Revision c53f30aa6fb6: /]

Firstly you must known the main concepts of ROS: nodes, topics, messages, etc. After this I recommend you to install the latest rosjava_core stack: http://www.ros.org/wiki/rosjava/Tutorials/Install. Then, to begin with rosjava is also recommendable to have a look to http://www.ros.org/wiki/rosjava/Overview.

After this you should make sure that you have compiled the java source code for the sensor_msgs/Image. To do this, you should understand before how the rosjava_bootstrap works. This package makes possible the java source code generation for any kind of messages. You can realize if you have the sensor message compiled checking if you have the folder for sensor_msgs:

cd  rospack find rosjava_bootstrap/target/classes/org/ros/message/sensor_msgs.

If not, as it was my case then you should generate the source code. We must compile the most essential message types for the correct building of rosjava: std_msgs, sensor_msgs and rosgraph_msgs

geus@geus-vaio:/opt/ros/external/packages/rosjava_core/rosjava_bootstrap$ rosrun rosjava_bootstrap java_msgs.py std_msgs sensor_msgs rosgraph_msgs geometry_msgs -o src/main/java/
generating messages for package [std_msgs]
generated messages for package [std_msgs] to [src/main/java/org/ros/message/std_msgs]
generating messages for package [sensor_msgs]
generated messages for package [sensor_msgs] to [src/main/java/org/ros/message/sensor_msgs]
generating messages for package [rosgraph_msgs]
generated messages for package [rosgraph_msgs] to [src/main/java/org/ros/message/rosgraph_msgs]
generated messages for package [geometry_msgs] to [src/main/java/org/ros/message/geometry_msgs]

Check if the java sourcecode is compiled:

geus@geus-vaio:/opt/ros/external/packages/rosjava_core/rosjava_bootstrap$ ls src/main/java/org/ros/message/
Duration.java  Message.java   rosgraph_msgs/ sensor_msgs/   Service.java   std_msgs/      Time.java

As we can see the sensor_msgs is available:

geus@geus-vaio:/opt/ros/external/packages/rosjava_core/rosjava_bootstrap$ ls src/main/java/org/ros/message/sensor_msgs/
CameraInfo.java      CompressedImage.java  Imu.java         JoyFeedbackArray.java  Joy.java        NavSatFix.java     PointCloud2.java  PointField.java  RegionOfInterest.java
ChannelFloat32.java  Image.java            JointState.java  JoyFeedback.java       LaserScan.java  NavSatStatus.java  PointCloud.java   Range.java

After this we can regenerate rosjava_bootstrap with:

rosmake rosjava_bootstrap rosjava

Now we can use the sensor_msg/Image type. We can use as a template of the [rosjava_tutorial_pubsub package] in the same rosjava_core stack. Here you are some example code for the subscriber:

public class Listener implements NodeMain {

  @Override
  public GraphName getDefaultNodeName() {
    return new GraphName("rosjava_tutorial_pubsub/listener");
  }

  @Override
  public void onStart(Node node) {
    final Log log = node.getLog();
    Subscriber<org.ros.message.sensor_msgs.image> subscriber =
        node.newSubscriber("chatter", "sensor_msgs/Image");
    subscriber.addMessageListener(new MessageListener<org.ros.message.sensor_msgs.image>() {
      @Override
      public void onNewMessage(org.ros.message.sensor_msgs.Image message) {
        log.info("I heard: \"" + message.data + "\"");
      }
    });
  }

  @Override
  public void onShutdown(Node node) {
  }

  @Override
  public void onShutdownComplete(Node node) {
  }
}

Then to launch it, we can follow the instruction explained on ROS wiki. Working over this tutorial code the only we have to do is recompiling the project and launch it with rosjava_bootstrap:

$ rosmake rosjava_tutorial_pubsub
[..]
$ rosrun rosjava_bootstrap run.py rosjava_tutorial_pubsub org.ros.tutorials.pubsub.Listener

Enjoy! I also did it as my first rosjava test 🙂

Vídeo Tutorial de Ros con Arduino

Ahí lo dejo para el que le pueda interesar como abordar la integración de ROS con arduino.

My robotic-books wish list

Ahí va una lista de libros interesantes para robótica que últimamente he leido/ quiero leer. Aquí la dejo para no olvidarla y por si a alguien le pudiera servir:
Robótica teórica y práctica:
Probabilistic Robotics (Intelligent Robotics & Autonomous Agents Series) de Sebastian Thrun, Wolfram Burgard y Dieter Fox (Tapa dura – 20 septiembre 2005)
Springer Handbook of Robotics by Bruno Siciliano and Oussama Khatib (Jun 27, 2008
Principles of Robot Motion: Theory, Algorithms, and Implementations (Intelligent Robotics and Autonomous Agents series)by Howie Choset, Kevin M. Lynch, Seth Hutchinson and George A. Kantor (May 20, 2005
Estos también he escuchado que son buenos de robótica más teórica:
Robotics: Modelling, Planning and Control (Advanced Textbooks in Control and Signal Processing) by Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani and Giuseppe Oriolo (Feb 11, 2011)
Springer Handbook of Automation by Shimon Y. Nof (Aug 27, 2009)
También otros muy interesantes, más prácticos:
Probabilistic Reasoning and Decision Making in Sensory-Motor Systems (Springer Tracts in Advanced Robotics) de Pierre Bessiere, Christian Laugier y Roland Y. Siegwart(Tapa blanda – 22 octubre 2010)
Arduino:
Arduino Robotics by John-David Warren, Josh Adams and Harald Molle (Jul 14, 2011)
Otros que a mi me interesan relacionados con robótica:
Computational Geometry: Algorithms and Applications by Mark de Berg, Otfried Cheong, Marc van Kreveld and Mark Overmars (Nov 19, 2010)

How to represent a 3D Gaussian Function with ROS rviz

The probabilistic functions are very usual in robotics to represent the certainty of states and observations. The most typical and used probabilistic function is the Gaussian function. Everybody who knows what is this immediately imagine a pretty bell which describes the samples nature.

In the one-dimensional case, a simple tuple (\mu, \sigma^{2}) mean, variance is enough information to imagine how the samples are behaving. This tuple can be also reinterpreted as an interval [\mu - \sigma^{2}, \mu + \sigma^{2}] where the samples most probably are located. For higher dimensions the \mu it is clear, but the equivalent element to describe the diversity, the covariance matrix \Sigma it is much lesser intuitive for most of people. The trick is imagine the ND gaussian bell as NDimensional ellipse. The shape and rotation of this ellipse is described by the covariance matrix. In this way the diversity of the distribution is intuitive.

It is extremely interesting to see and understand the relationship with the rotation and scale of the ellipsoid. If we define R as the rotation transform of the ellipsoid and l is a vector with the variances in the local frame of the ellipse (much easier to imagine) l = \begin{pmatrix}\sigma_{x_1,x_1}^{2} & ... & \sigma_{x_n,x_n}^{2}\end{pmatrix} , then the relationship between R, l and \Sigma is this:

\Sigma = R (l I) R^{t}

This have important implications because if you want to synthesizer a gaussian belief from a theoretical model can be hard imagine the covariance matrix values but it is very easy to think in an ellipse with a rotation and scale.

The inverse operation is also useful. For instance if you have a covariance matrix obtained experimentally and you wish to make a geometric interpretation of the distribution or just plot it, you can obtain the rotation matrix R and the scale vector l. Then the it is needed to compute the eigenvectors and eigenvalues. That is because:

R = eigvectors(\Sigma)
and
l = eigvalues(\Sigma)

Of course notice that if number of eigenVectors is lower than the dimensionality of the problem is because the ellipse is infinitely slim and as a consequence the determinant det \left | \Sigma \right | = 0 and then the matrix is singular. This means that one dimension should be removed if you want to compute likelihoods over that gaussian function.

Here I show how to represent a 3D gaussian function with ROS and rivz:

And here the code in python. Discussion about this subject in: http://answers.ros.org/question/2043/plot-a-gaussian-3d-representation-with-markers-in:

import roslib
roslib.load_manifest ("gaussian_markers")
import rospy
import PyKDL
from visualization_msgs.msg import Marker, MarkerArray
import visualization_msgs
from geometry_msgs.msg import Point
import numpy
from numpy import concatenate

#syntetic 3-D Gaussian probabilistic model for sampling
covMatModel =[[10, 10, 0],[10, 1, 0],[0, 0, 4]]
meanModel =[10, 10, 10]

#used to paint the autovectors
def markerVector(id,vector,position):
	marker = Marker ()
	marker.header.frame_id = "/root";
	marker.header.stamp = rospy.Time.now ()
	marker.ns = "my_namespace2";
	marker.id = id;
	marker.type = visualization_msgs.msg.Marker.ARROW
	marker.action = visualization_msgs.msg.Marker.ADD
	marker.scale.x=0.1
	marker.scale.y=0.3
	marker.scale.z=0.1
	marker.color.a= 1.0
	marker.color.r = 0.33*float(id)
	marker.color.g = 0.33*float(id)
	marker.color.b = 0.33*float(id)
	(start,end)=(Point(),Point())

	start.x = position[0]
	start.y = position[1]
	start.z = position[2]
	end.x=start.x+vector[0]
	end.y=start.y+vector[1]
	end.z=start.z+vector[2]

	marker.points.append(start)
	marker.points.append(end)
	print str(marker)
	return marker

rospy.init_node ('markersample', anonymous = True)
points_pub = rospy.Publisher ("visualization_markers", visualization_msgs.msg.Marker)
gauss_pub = rospy.Publisher ("gaussian", visualization_msgs.msg.Marker)

while not rospy.is_shutdown ():
	syntetic_samples = None

	#painting all the syntetic points
	for i in xrange (10, 5000):
		p = numpy.random.multivariate_normal (meanModel, covMatModel)
		if syntetic_samples == None:
			syntetic_samples =[p]
		else:
			syntetic_samples = concatenate ((syntetic_samples,[p]), axis = 0)

		marker = Marker ()
		marker.header.frame_id = "/root";
		marker.header.stamp = rospy.Time.now ()
		marker.ns = "my_namespace2";
		marker.id = i;
		marker.type = visualization_msgs.msg.Marker.SPHERE
		marker.action = visualization_msgs.msg.Marker.ADD
		marker.pose.position.x = p[0]
		marker.pose.position.y = p[1]
		marker.pose.position.z = p[2]
		marker.pose.orientation.x = 1
		marker.pose.orientation.y = 1
		marker.pose.orientation.z = 1
		marker.pose.orientation.w = 1
		marker.scale.x = 0.05
		marker.scale.y = 0.05
		marker.scale.z = 0.05
		marker.color.a = 1.0
		marker.color.r = 0.0
		marker.color.g = 0.0
		marker.color.b = 0.0
		points_pub.publish (marker)

	#calculating Gaussian parameters
	syntetic_samples = numpy.array (syntetic_samples)
	covMat = numpy.cov (numpy.transpose (syntetic_samples))
	mean = numpy.mean ([syntetic_samples[: , 0], syntetic_samples[: , 1], syntetic_samples[:, 2]], axis = 1)

	#painting the gaussian ellipsoid marker
	marker = Marker ()
	marker.header.frame_id ="/root";
	marker.header.stamp = rospy.Time.now ()
	marker.ns = "my_namespace";
	marker.id = 0;
	marker.type = visualization_msgs.msg.Marker.SPHERE
	marker.action = visualization_msgs.msg.Marker.ADD
	marker.pose.position.x = mean[0]
	marker.pose.position.y = mean[1]
	marker.pose.position.z = mean[2]

	#getting the distribution eigen vectors and values
	(eigValues,eigVectors) = numpy.linalg.eig (covMat)

	#painting the eigen vectors
	id=1
	for v in eigVectors:
		m=markerVector(id, v*eigValues[id-1], mean)
		id=id+1
		points_pub.publish(m)

	#building the rotation matrix
	eigx_n=PyKDL.Vector(eigVectors[0,0],eigVectors[0,1],eigVectors[0,2])
	eigy_n=-PyKDL.Vector(eigVectors[1,0],eigVectors[1,1],eigVectors[1,2])
	eigz_n=PyKDL.Vector(eigVectors[2,0],eigVectors[2,1],eigVectors[2,2])
	eigx_n.Normalize()
	eigy_n.Normalize()
	eigz_n.Normalize()
  	rot = PyKDL.Rotation (eigx_n,eigy_n,eigz_n)
	quat = rot.GetQuaternion ()

	#painting the Gaussian Ellipsoid Marker
	marker.pose.orientation.x =quat[0]
	marker.pose.orientation.y = quat[1]
	marker.pose.orientation.z = quat[2]
	marker.pose.orientation.w = quat[3]
	marker.scale.x = eigValues[0]*2
	marker.scale.y = eigValues[1]*2
	marker.scale.z =eigValues[2]*2

	marker.color.a = 0.5
	marker.color.r = 0.0
	marker.color.g = 1.0
	marker.color.b = 0.0

	gauss_pub.publish (marker)
	rospy.sleep (.5)

Libros para la introducción e investigación en Robótica

Como los que me conocéis sabéis, actualmente soy estudiante de Doctorado en el departamento ATC (Arquitectura y Tecnología de computadores) en E.T.S. Ingeniería Informática de Sevilla. La temática de mi tesis se ubica dentro del área de la robótica. Actualmente me encuentro en la fase de realización de la tesina (trabajo de investigación previo). Básicamente consiste en la realización de un estudio profundo del estado del arte , detectar carencias en las soluciones propuestas e introducir las líneas generales del trabajo de Tesis. Dentro de la robótica (por interés personal y por historia de la investigación en el departamento) me centro en las áreas de navegación, planificación, control compartido, sensores distribuidos y arquitecturas software robóticas.

Los artículos de investigación (Papers) son muy útiles para estar a la última, sin embargo la cantidad de papers que aparecen cada mes es desorbitarte, hay mucha información que no es relevante y en ocasiones es difícil discernir si las aportaciones son buenas, interesantes para tu investigación y merece la pena una profundización.

Sin embargo existe una fuente de información complementaria e igual o más importante que los papers: los libros. Los libros habitualmente están más atrasados que los papers, sin embargo, estos presentan innumerables cualidades: se profundiza mucho más en la temática, formato y notación unificada, aprendizaje progresivo, etc. Además, los libros ofrecen contenidos habitualmente más aceptados y reconocidos por la comunidad científica con una madurez media o alta. De esta forma se convierten en un pilar básico para la formación alrededor de los cuales debe girar el conocimiento adquirido en los artículos científicos (más efímeros y traicioneros). Aquí dejo una lista de los 4 libros (hay alguno más que me encantaron, pero estos son los principales) que para mí son sobresalientes y que están permitiéndome como estudiante de doctorado con vagos conocimientos previos en la robótica ir adquiriendo poco a poco destreza y experiencia como para poder aportar nuevas ideas.

Probabilistic Robotics – Año 2006. Un libro absolutamente imprescindible para comprender los enfoques probabilísticos que utiliza hoy día la robótica para aportar soluciones. Tiene una matemática dura, y todas las técnicas presentadas ofrecen su demostración matemática. En cualquier caso es un libro muy aplicable de manera casi directa en el software para robots. Se centra en las áreas de localización, construcción de mapas, modelos de sensores y modelos de movimiento de robots y planificación, todo desde un enfoque probabilístico. A pesar de explicar diversas técnicas exitosas en estas áreas el enfoque del libro no son estas sino más bien pretende ilustrar un punto de partida para enfocar los problemas robóticos de una forma mas realista y robusta haciendo uso de la estadística. Su principal autor Sebastian Thrun es hoy día una de las principales referencias mundiales en el área de la robótica.
Planning Algorithms – Año 2010. Este libro profundiza de forma exhaustiva en muchísimas técnicas y aspectos importantes en la planificación para robot. Se centra especialmente en la planificación de movimiento (aunque también trata temas de planificación de más alto nivel, tareas, etc). Es muy profundo, duro y formal matemáticamente. Más específicamente se centra en técnicas de planificación de movimiento global deterministas y basadas en muestras, planificación con restricciones kinodinámicas y no-holónomas, etc. Una de las principales ventajas de este libro es que está disponible online de forma gratuita, y además que es actualizado frecuentemente.
Springer Handbook of Robotics – La biblia de la robótica. Casi 1600 páginas de conocimiento. Bastante actualizado, finales del año 2008. Libro para ser utilizado como referencia para introducirse a cualquier temática de la robótica y encontrar fuentes. Trata de todos los temas: Fundamentos de la robótica (Cinemática, Dinámica, Mecánica, Control, Planificación, IA, Arquitecturas software), Estructuras de Robots, Sensores y percepción, manipulación, robótica distribuida, robótica de servicios, robótica en el ámbito de los seres humanos. El libro ha sido realizado por decenas de autores, especialistas en cada área. El nivel de formalismo y profundización varía según el capítulo y la madurez del tema. Otro libro muy recomendable para tener a mano.

Principles of Robot Motion – Este libro se centra en la planificación de movimiento. Es un libro cuya temática es muy parecida a Planning Algorithms, sin embargo es mucho mas ameno y sencillo de leer especialmente para los Ingenieros Informáticos (como yo) ya que presenta vocabulario y técnicas familiares del área de la IA. Realiza un estudio del arte (año 2005) en técnicas de planificación, centrándose en la planificación global (incluido sistemas con restricciones kinodinámicas). Toca también temas de cinemática, dinámica, técnicas de localización probabilísticas y SLAM.

Pues nada, ahí lo dejo, espero que pueda ayudar a cualquiera que esté iniciándose e investigando en este área. ¡Cualquier comentario, sugerencia u opiniones sobre estos u otros libros de robótica son bienvenidos!
Saludos. Bon apetit!

[First Lego League] Competición de Robots de Lego en la E.T.S.I.Informática

El domingo 6 de Marzo (dentro de 16 días) se realizará en la escuela un evento de robótica entrañable y divertido! Participarán 24 equipos de niños y adolescentes de entre 10 y 16 años de institutos de Sevilla. Los equipos diseñan robots haciendo uso del robot LEGO NXT que deben superar diversas pruebas en un terreno de juego conocido.

¡¡Se trata de un día de jornadas abiertas en la escuela!! ¡¡Por lo tanto estáis todos invitados a venir!! Primos y hermanos, pequeños y mayores aficionados de Lego y Robots pueden disfrutar como enanos viendo los distintos robots enfrentándose a las pruebas!

La “First Lego League” es un torneo internacional de robótica para niños que se desarrolla en un ambiente deportivo y festivo (padres, amigos, música, speakers, cámaras, etc.. como una party). Se trata de un evento donde el principal objetivo es despertar en el interior de los niños y adolescentes el interés e inquietud por la ciencia y la ingeniería . Todo en un ambiente sano, divertido y donde se premian los valores humanos como la amistad, el esfuerzo, el trabajo en equipo y el respeto.

El evento consta de tres competiciones:

  • Competición de robots LEGO real. (Los robots deberán mover, empujar, golpear o transportar objetos)
  • Presentación de un proyecto técnico
  • Presentación de un proyecto científico

Adjunto un vídeo del mismo torneo realizado en Barcelona para que veas sobre qué estoy hablando:  . Los robots son diseñados usando LEGO NXT (robots para niños) y todos ellos disponen ya de las mesas de pruebas donde competirán.

Vídeo de un SuperRobot de lego Noruego haciendo las pruebas de este año!!

Monodimensional UKF – Unscented Kalman Filter in Python

El método “Unscented Kalman Filter” es un importante método de estimación recursivo gaussiano. Este tipo de técnicas son útiles para estimar el estado de un robot integrando la información proveniente de las medidas de los sensores con el conocimiento previo que tenemos sobre el sistema (modelo teórico).

En la robótica probabilística, el estado no es conocido con precisión. Por ejemplo, la posición de un robot (x,y,z) (que podría ser descrito por una tupla de tres reales) es descrita por una función probabilística tridimensional. Esta función que describe el estado se denomina en la literatura creencia bel(x). De esta forma la posibilidad de que un robot se encuentre en un estado determinado queda definida estocásticamente. Este enfoque puede describir de una forma mas realista el conocimiento que tenemos sobre el estado. Además permite otras ventajas como mantener múltiples hipótesis sobre el estado del robot. Por ejemplo, un robot se encuentra en la mezquita de córdoba y detecta con sus sensores que se encuentra delante de una columna, sin embargo si el estado en que se encontraba anteriormente no describía una mejor hipótesis clara, esta columna podría ser cualquiera de las cientos de columnas existentes en la mezquita.

La dos gráficas ilustran el comportamiento típico de los métodos de estimación recursivos gaussianos. En concreto este  ejemplo ha sido generado a partir del método UKF. Supongamos que el estado es monodimensional (por ejemplo la posición en el eje x):

La primera figura muestra en azul la creencia en el instante anterior del estado del robot. bel[t-1](x) Se observa como se cree que su valor es aproximadamente uno. Sin embargo el valor real podría ser 1.1 (representado mediante la línea vertical roja).

Supongamos que pasa un instante de tiempo. Haciendo uso del conocimiento teórico (modelo matemático) que tenemos del sistema y dadas las entradas conocidas, predecimos que el estado en el instante actual será alrededor de 3 (función amarilla). Esta función de probabilidad se denomina creencia anterior (predicción) ^bel[t](x). Dado que nuestro modelo es teórico  es evidente que tiene un error. Este error puede ser típicamente acotado teóricamente o mediante métodos empíricos en forma de matriz de covarianza. Por lo tanto podemos estimar que la creencia anterior ^bel[t](x) tendrá una mayor desviación típica que la creencia del instante anterior bel[t-1](x). Es decir, tras la predicción, tenemos una mayor incertidumbre del estado del robot. La línea vertical roja representa la posible posición real del robot en el estado actual.

A continuación recibimos las medidas del sensor. Estas medidas nos indican el estado de forma total o parcial (de forma directa o tras algún tipo de de procesamiento). Pero también, los sensores presentan un error (típicamente indicado por el fabricante o también se podría obtener mediante métodos empíricos).  En la segunda figura, la función de probabilidad roja representa tal medida y su error. En la misma también se muestra en azul la creencia anterior ^bel[t](x) predicha por el modelo teórico.

El método UKF (al igual que KF, EKF y otros) nos permite estimar a partir de los errores del modelo teórico y de la medida, en cual de ambos debemos confiar más, obteniendo un factor de ganancia K para ponderar ambas funciones de probabilidad. Una vez obtenido este factor de ganancia K se puede calcular la creencia posterior (final) del instante actual bel[t](x). La función discontinua verde representa tal creencia.

Para una descripción matemática mas detallada del funcionamiento del método UKF:
http://www.cslu.ogi.edu/nsel/ukf/node6.html

A continuación muestro una implementaicón personal en python del método UKF acompañada en la simulación utilizada en este ejemplo:

# This code is subject under the terms of the BSD License
# 2011 - Pablo Iñigo Blasco
# http://techbase.kde.org/Policies/Licensing_Policy#BSD_License</p>
import scipy.stats
from numpy import *
from matplotlib.pyplot import *

def getSigmaPoints(mu, sigma, n=1., alpha=1., kappa=1., betta=2.):
 #space for sigma points
 X = zeros(2 * n + 1)
 wm = zeros(2 * n + 1)
 wc = zeros(2 * n + 1)
 lambd = alpha * alpha * (n + kappa) - n

 #calculate sigma points
 X[0] = mu
 X[1] = mu + (math.sqrt((n + lambd) * sigma))
 X[2] = mu - (math.sqrt((n + lambd) * sigma))

 #calculate weights
 #first weight
 wm[0] = lambd / (n + lambd)
 wc[0] = lambd / (n + lambd) + (1 - alpha * alpha + betta)

 #rest of weights
 wm[1] = wc[1] = 1 / (2. * (n + lambd))
 wm[2] = wc[2] = 1 / (2. * (n + lambd))

 return [X, wm, wc]

def process_state_transition(x, u):
 return u +  x*x

def measurement(x):
 return x;

def measurement_noise(measurement_cov):
 return scipy.stats.norm.rvs(loc=0, scale=measurement_cov, size=1)

def process_noise(process_cov):
 return scipy.stats.norm.rvs(loc=0, scale=process_cov, size=1)

def h(x):
 return x;

#UKF parameters
alpha = 1.;
kappa = 0.;
betta = 2.;
n = 1;

#model parameters
#process noise covariance matrix
R = 0.45
Q = 0.3

#suppose the control input u = 1
u = 2;

#previous state believe
mu_prev = 1;
sigma_prev = 0.1;

real_prior_state = scipy.stats.norm.rvs(loc=mu_prev, scale=sigma_prev, size=1)
print "real previous state %f"%(real_prior_state)
print "posterior belief of the previous state (mu=%f,sigma=%f)"%(mu_prev,sigma_prev)

f = scipy.stats.norm(mu_prev, sigma_prev)
#getting sigma points from previous state belief
[X_prior_state, wm, wc] = getSigmaPoints(mu_prev, sigma_prev, n, alpha, kappa, betta)

#plot this previous state belief
#http://matplotlib.sourceforge.net/users/pyplot_tutorial.html
abcises = linspace(-1, 5, 400)
pX = f.pdf(X_prior_state)
figure(1)
subplot(211)
#plot gaussian function
plot(abcises, f.pdf(abcises),"b")
#plot sigma points
plot(X_prior_state, pX, "r+")
show();

#plot real previous state
axvline(real_prior_state,color="r")

print "...process happening..."
real_current_state = process_state_transition(real_prior_state,u) +  process_noise(R);
print "real current state %f"%(real_current_state)

#applying the process_state_transition to all sigma points to later build the prior belief
X_current_state = [process_state_transition(x, u) for x in X_prior_state]
plot(X_current_state, pX, "b+")

#rebuild the gaussian function after applying the process state transition to the sigma points samples
prior_bel_mu = sum(wm*X_current_state);
prior_bel_sigma_without_noise = sum (wc * (X_current_state - prior_bel_mu)* (X_current_state - prior_bel_mu))
prior_bel_sigma = prior_bel_sigma_without_noise + R

prior_bel_f_without_noise = scipy.stats.norm(prior_bel_mu, prior_bel_sigma_without_noise)
prior_bel_f = scipy.stats.norm(prior_bel_mu, prior_bel_sigma)
print "prior belief (mu=%f,sigma=%f)"%(prior_bel_mu,prior_bel_sigma)

#plot the prior belief after the process state transition without noise
#plot(abcises, prior_bel_f_without_noise.pdf(abcises),"g")
#plot the prior belief (with noise)
plot(abcises, prior_bel_f.pdf(abcises),"y")
#plot real current state
axvline(real_current_state,color="r")
subplot(212)
#also plot it in the bottom frame
axvline(real_current_state,color="r")

#so now our best guess is the prior belief. Then we get its sigma points
[X_prior_bel, wm, wc] = getSigmaPoints(prior_bel_mu, prior_bel_sigma, n, alpha, kappa, betta)

#so given the sigma points hypotesys, we get the expected measurements given our measurement model and noise
#in some way this is a substate of the prior_belief
Z_expected = [h(x) for x in X_prior_bel]
z_mu_expected = sum(wm*Z_expected);
z_sigma_expected_without_noise = sum (wc * (Z_expected - z_mu_expected)* (Z_expected - z_mu_expected))
z_sigma_expected = z_sigma_expected_without_noise + Q;
print "expected measurement (mu=%f,sigma=%f)"%(z_mu_expected,z_sigma_expected)

#plot the expected measurement given our best state guess (prior_belief)
#observe how the quality in this expectation depends on the prior belief convariance which
#at the same time depends on process covariance R
#In any case the expected measurement can be "more precise" or "less precise" that the prior belief
plot(abcises, scipy.stats.norm(z_mu_expected,z_sigma_expected).pdf(abcises),"b")

# calculate the gain, who we trust: process or measurement
K = sum(wc*(X_prior_bel - prior_bel_mu)*(Z_expected - z_mu_expected))/z_sigma_expected
print "Gain: %f"%K

# if K gets 1 trust the measurement
# if K gets 0 trust the process state transition model

#simulate observation
z = measurement(real_current_state) + measurement_noise(Q);
print "measurement %f"%(z)
plot(abcises, scipy.stats.norm(z,Q).pdf(abcises),"r")

#calculate the posterior gain
post_bel_mu = prior_bel_mu + K * h(z - z_mu_expected)
post_bel_sigma = prior_bel_sigma - K * z_sigma_expected *K

print "posterior belef (mu=%f,sigma=%f)"%(post_bel_mu,post_bel_sigma)
plot(abcises, scipy.stats.norm(post_bel_mu,post_bel_sigma).pdf(abcises),"g--")