Semana 38-39. Investigación de la librería OMPL y montaje del vídeo de presentación de los ejercicios Follow Person
Ejercicio Amazon Warehouse.
Librería OMPL
Para la resolución de la navegación global del ejercicio Amazon Warehouse se va a utilizar la librería OMPL, una librería diseñada para solucionar problemas de planificación de movimientos enfocada en algoritmos basados en muestreo.
La librería permite dos tipos de planificación: planificación geométrica sin considerar controles y planificación considerando controles. Y los componentes principales de la líbrería son:
- Espacio de estado
- Espacio de control para sistemas dinámicos
- Sampler
- Comprobador de validez de estado (StateValidityChecker)
- Propagador de estado (StatePropagator), solo para problemas con espacio de control
- Planificador
La librería por sí no tiene implementado ningún comprobador de validez de estado, por lo que es tarea del usuario en implementarlo teniendo total libertad en el diseño.
En líneas generales los pasos que hay que seguir para la solución de un problema sin considerar los controles es: 1) Definir el espacio de estado del problema y crear el contexto de planificación. Por ejemplo,
from ompl import base as ob
from ompl import geometric as og
space = ob.RealVectorStateSpace()
ss = og.SimpleSetup(space)
2) Implementar la función de comprobación de la validez de estado
def isStateValid(self, state):
w = min(int(state[0]), self.maxWidth_)
h = min(int(state[1]), self.maxHeight_)
c = self.im[h, w]
return c[0] > 250 and c[1] > 250 and c[2] > 250
3) Definir el estado inicial y final
start = ob.State(self.ss_.getStateSpace())
start()[0] = start_row
start()[1] = start_col
goal = ob.State(self.ss_.getStateSpace())
goal()[0] = goal_row
goal()[1] = goal_col
self.ss_.setStartAndGoalStates(start, goal)
Además, también se puede elegir diferentes planificadores
ss.setPlanner(og.PRM(self.ss_.getSpaceInformation()))
Solucionar el problema
ss.solve()
Y obtener la ruta solución
p = ss.getSolutionPath()
En el caso de que se considere los controles habría que hacer pasos extras:
4) Definir el espacio de control
from ompl import control as oc
cspace = oc.RealVectorControlSpace(space, 2)
5) Implementar el propagador de estado indicando cómo evoluciona el sistema tras aplicar un control
def propagate(start, control, duration, state):
state.setX(start.getX() + control[0] * duration * cos(start.getYaw()))
state.setY(start.getY() + control[0] * duration * sin(start.getYaw()))
state.setYaw(start.getYaw() + control[1] * duration)
Tutoriales y Demos
Para entenderlo mejor he seguido algunos tutoriales y probado algunas demos que podría ser de interés para el ejercicio de Amazon Warehouse.
- Ejemplo Point2DPlanning. Se define como estado de espacio válido aquellos píxeles de color blanco de la imagen del mapa. El mapa es una imagen PPM y utiliza la clase PPM que tiene implementada. Pero en nuestro caso podemos utilizar la imagen PNG y utilizar al librería OpenCV como ayuda.
Solution Path: Geometric path with 8 states RealVectorState [0 0] RealVectorState [470.369 211.364] RealVectorState [704.249 458.141] RealVectorState [701.618 490.472] RealVectorState [728.469 975.888] RealVectorState [693.098 1094.16] RealVectorState [835.329 1131.33] RealVectorState [777 1265]
- Ejemplo KinematicCarPlanning. En este ejemplo se define una serie de obstáculo y comprueba que el estado no cae dentro de dicho área. Además, el espacio de estado es una combinación de espacio de translación (RealVectorState) y espacio rotación 2D (S02State) mientras que en el ejemplo anterior solo consta del espacio de traslación.
Soluction path: Geometric path with 10 states
Compound state [
RealVectorState [0 0]
SO2State [0.785398]
]
Compound state [
RealVectorState [1.38769 0.710116]
SO2State [0.712748]
]
Compound state [
RealVectorState [3.31573 1.6731]
SO2State [0.72219]
]
...
Compound state [
RealVectorState [8.93291 8.03933]
SO2State [0.782388]
]
Compound state [
RealVectorState [9.19385 8.48612]
SO2State [0.781776]
]
Compound state [
RealVectorState [10 10]
SO2State [0.785398]
]
- Ejemplo RigidBodyPlanningWithControls. En este tercer ejemplo se tiene en cuenta el espacio de control; el estado inicial y final no solo incluye las coordenadas sino también la guiñada del robot.
Found solution:
Control path with 33 states
At state Compound state [
RealVectorState [-0.5 0]
SO2State [0]
]
apply control RealVectorControl [0.103329 0.171441]
for 6 steps
At state Compound state [
RealVectorState [-0.438086 0.00265528]
SO2State [0.102865]
]
apply control RealVectorControl [0.0679152 -0.00458726]
for 9 steps
At state Compound state [
RealVectorState [-0.377274 0.00882011]
SO2State [0.0987364]
]
apply control RealVectorControl [-0.115119 0.229902]
for 1 steps
At state Compound state [
RealVectorState [-0.38873 0.00768531]
SO2State [0.121727]
]
apply control RealVectorControl [0.272367 -0.297622]
for 1 steps
...
Posible solución para el ejercicio Amazon Warehouse
Una posible solución para nuestro ejercicio es basandonos en el primer ejemplo y definir el espacio de estado como espacio de translación; engordamos los bordes de los obstáculos para que sea mayor que el radio del robot. A continuación, muestro algunas pruebas realizadas con diferentes algoritmos. El punto verde es el estado inicial y el rojo el final.
1) Algoritmo PRM (Probabilistic Roadmap)
Se genera muestras aleatorias por el estacio de estado de forma uniforme y luego se connectan para generar diferentes caminos.
Info: PRM: Starting planning with 2 states already in datastructure
Info: PRM: Created 194 states
Info: Solution found in 0.252841 seconds
Info: SimpleSetup: Path simplification took 0.019760 seconds and changed from 9 to 6 states
Soluction path: Geometric path with 6 states
RealVectorState [10 10]
RealVectorState [12.8026 68.7174]
RealVectorState [16.6385 101.467]
RealVectorState [132.473 142.14]
RealVectorState [136.438 191.023]
RealVectorState [211 233]
2) Algoritmo RRT (Rapidly-exploring Random Tree)
A diferencia con el anterior, este algoritmo va generando muestras gradualmente empezando por los alredores del estado inicial y poco a poco se va extendiendo hasta alcanzar el estado final.
Debug: RRT: Planner range detected to be 99.735851
Info: RRT: Starting planning with 1 states already in datastructure
Info: RRT: Created 10 states
Info: Solution found in 0.005425 seconds
Info: SimpleSetup: Path simplification took 0.029798 seconds and changed from 9 to 6 states
Soluction path: Geometric path with 6 states
RealVectorState [10 10]
RealVectorState [13.3645 90.1881]
RealVectorState [15.3647 95.2637]
RealVectorState [184.809 138.689]
RealVectorState [193.372 162.205]
RealVectorState [211 233]
Se puede observar que las soluciones generadas con el primer algoritmo no son tan estables como en el algoritmo RRT.
Vídeo presentación de Follow Person
Follow Person Simulado