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)

El debate energético y la bajeza moral de occidente

A cuento del debate energético y la cuestión de ¿Por qué son caras las energías renovables? y la disparidad de argumentaciones que están apareciendo ahora con el problema nuclear Japonés de moda, un buen amigo pregunta las siguientes cuestiones:

1. Costes (hablo únicamente de dinero). ¿Qué energía es más barata, cuanto y porqué? ¿Cuantos nos costaría una factura de electricidad únicamente renovable? El tema de las subvenciones.. me parece un auténtico lío.

2. ¿Qué es y como funciona la subasta energética?

3. ¿Podría sostenerse solo un país con energía renovables? ¿solo para la producción de electricidad? En el caso de que se pueda, ¿podemos asumir eso?

4. Si las eléctricas han estado vendiendo por debajo del coste tantos años (es lo que han dicho cuando la subida de la luz), ¿porque han ganado lo que han ganado? ¿porqué ha estado congelado el precio de electricidad tantos años?

En fin, te pones a buscar por ahí y encuentras una cosa y la contraria..

Son en mi opinión, preguntas equivocadas.

El dinero en general, y en especial el dinero que nosotros pagamos en la factura de la luz y la indignación que despierta en nosotros no es más que otro ejemplo del mito de la caverna de Platón (una simple proyección de la compleja realidad). Realizar cualquier argumentación a favor o en contra de una energía basada en lo que pagamos en la factura de la luz es -en mi opinión- absolutamente simplista.

Independientemente de que algún día las renovables puedan ser más rentables o no bajo este enfoque simplista – algo que ignoro – Se puede intuir que con las reglas y mentalidad actuales es difícil que las renovables encuentren su lugar y que hagan más barata nuestra factura. Principalmente porque la mayoría de las energías no-renovables son mucho más competitivas ya que externalizan costes de una forma brutal…y de esta forma, a nuestra visión simplista son más atractivas y más baratas para nuestro bolsillo.

Pero una cosa tengo clara. Las energías renovables forman parte de un CICLO de vida económico, energético y social muchísimo más limpio, transparente, JUSTO y sostenible. En ese ciclo cerrado se puede ver de forma clara que hay un trato justo y respetuoso con las personas y hacia el entorno. Y donde las mismas sociedades que realizan el esfuerzo de cultivar, recogen los beneficios. No hay sociedades explotadas y explotadoras. Recogemos lo que sembramos. Creo que eso es una ley universal que nunca deberíamos olvidar (y hoy día en occidente lo hemos hecho).

Hay algo más allá. Nosotros hemos estado “comprando” con nuestro silencio hipócrita-democrático injusticias, dictadores y opresión en buena parte del mundo (incluida Libia) por el mero y simple hecho de la estabilidad, por interés de nuestros mercados. Nuestros mercados, nuestro Santo Grial. Es decir, hemos estado escupiendo a los valores de los derechos humanos y la democracia. ¿Donde está nuestra dignidad? Occidente, potencia cultural, tecnológica y diplomática. Nosotros deberíamos ser los defensores de los valores y derechos humanos. Una vergüenza, … a su vez una bomba de relojería para nosotros mismos.

Nuestra dignidad humana no tiene precio, y la estamos vendiendo. Y nunca podremos tener la legitimidad de exigir (y nunca habrá) justicia, igualdad y derechos humanos, cuando nosotros con nuestro silencio interesado fomentamos lo opuesto.

En mi opinión, sin lugar a dudas las renovables pueden tecnológicamente mantener un país y lo están demostrando. Es cierto que presenta nuevos retos tecnológicos y sociales. Pero es posible. España en concreto tiene un potencial renovable brutal, una situación de cabeza, una oportunidad para mejorar (no ya económicamente) sino humanamente. Oportunidad que no podemos ni debemos perder.

Volviendo a lo que pagamos en nuestra factura. Parto de que las empresas ganan de lo que nos quitan. Si nos quitan más, más ganan. Nos mentirán hasta la saciedad para mantener o mejorar su posición. Y si pueden intentarán engañarnos y a la vez lograr una sonrisa en nuestra cara y agradecimiento (como en las rebajas de enero). Incluido el lobby renovable. Pero superar esto es para nota. Pero como sociedad tenemos que mirar que haríamos según nuestras necesidades, no según las necesidades de las corporaciones.

Pienso que la energía es cara (sí, es solo una intuición). Porque si las renovables son “caras” (conociendo su ciclo de vida transparente) y la gasolina está como está (conociendo su forma brutal de externalizar) y viendo la que se está liando en Japón… la energía no debe de ser barata… La energía debe valorarse. Esto implica que tenemos que pagar más por la energía, pagar más por el I+D+I en nuevas energías. Y sin lugar a dudas tenemos que cambiar nuestra forma de consumir energía.

Estoy seguro que con tecnología, implicación social y legal las renovables explotarán, madurarán y acabarán convirtiéndose en un símbolo de justicia y un tesoro de valor incalculable para nuestra sociedad.

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--")

Panini de Calabacín

Ingredientes (para 2 paninis):

– Calabacín mediano
– Cebolla grande
– Atún
– Ajo
– Queso
– Pimiento
– una andaluza

Se pica un calabacín, una cebolla y tres dientes de ajos en trozos muy pequeñitos. Se sofrie lentamente con un chorreón de aceite de oliva, se le añade un puñado de pimiento picado (yo uso el congelado del mercadona). Cuando están las verduras un poco blanditas (2 o 3 minutos a fuego medio) se añaden dos latas de atún al sofrito y una hoja de laurel partida en trocitos muy pequeños (quitando el tallo de la hoja y otras durezas). Orégano al gusto.

Antes de que se dore se saca y se distribuye sobre dos rebanadas de andaluzas o barrita. Yo las suelo tener congeladas por lo que primero las tuesto un poco. Luego se raya queso encima de los paninis y se les echa un poco mas de orégano encima. Finalmente de 10 a 15 minutos al horno a 260 grados al gusto.

 

 

Testing Kinect and ROS

This afternoon I’ve been playing a bit with Kinect and ROS. The implemented software by openni and the ROS team is quite easy to use and the result are impressive. Our Group (RTC Robotic and Computer Technology from the Universidad de Sevilla) is interested in Kinect to bring to our robots (mainly for eldery care and rehabilitation proposes)the capability to understand better their environment: better navigation and SLAM, better Human-Robot Interaction, etc.

Here you can watch some basic samples:

Video conversion: from ogv to avi, and other video editing tricks

Convert video

sudo apt-get install mencoder
mencoder input.ogm -ovc xvid -oac mp3lame -xvidencopts pass=1 -o output.avi

Sometimes it is also interesting to convert the video at slow or fast motion. This is possible adding the parameters -speed. For instance -speed 1/4 or -speed 2

Cut video
To cut and select a set of frames from a video and convert them to another video or to a set of images use:

avconv -ss 00:00:11 -t 00:00:07 -i ./Ejemplo\ tiras\ imagenes.ogv -vf crop=500:500:200:100 -f image2 image%03d.jpeg

Ubuntu 10.10.
Ubuntu 12.10. [retested]

Regards.