Tutorial:ODE y robots modulares:Caida libre (II)

De WikiRobotics
Saltar a: navegación, buscar
Tutorial ode box2 1.png

Simulación de un cubo en caida libre (II): Colisiones

Capítulo anterior
Índice
Capítulo siguiente

Introducción

En este ejemplo partiremos de la simulación del cubo en caida libre presentada en el capítulo anterior, añadiremos el suelo e implementaremos la detección de colisiones. Ahora el cubo chocará contra el suelo y permanecerá sobre él hasta el final de la simulación. Se presentan los nuevos parámetros que hay que definir para la realización de las colisiones y código necesario para su implementación.

Objetivos

  • Incorporar detección de colisiones en el mundo virtual

Código

Programa principal
Definición de las constantes.

Compilación

Todos los ejemplos de este tutorial compilan tecleando "make". Sin embargo se describe a continuación cómo se compila directamente usando el GCC:

g++  -Iinclude  -o box2 box2_ex/box2.cpp -lm -lode

Ejecución

Resultado de la simulación del ejemplo box2

Para su ejecución redireccionaremos la salida estándar hacia el fichero z.m para poder visualizarlo con octave:

./box2 > z.m
octave z.m

La gráfica resultante se puede ver en la figura de la derecha. En el eje horizontal se representa el tiempo, en tics de 5ms. En el vertical la altura del cubo (coordenada z), que va disminuyendo con el tiempo.

Al ejecutar el ejemplo, puede aparecer el siguiente mensaje de error:

ODE Message 3: LCP internal error, s <= 0 (s=-0.0000e+00)

Es un mensaje emitido por el ODE para indicara que ha ocurrido un error interno al realizar ciertos cálculos, pero la simulación se realiza correctamente. Se puede encontrar maś información sobre su significado en esta entrada de las FAQ del ODE.

Conceptos sobre colisiones

Cuerpos y geometrías

Los objetos que se simulan en ODE constan de dos elementos:

  • Cuerpos (bodies): Son los elementos de simulación del ODE. Se comportan como sólidos rígidos. En sus propiedades se definen todas las magnitudes cinemáticas y dinámicas: vector de posición, orientación, velocidades lineales y angulares, aceleraciones lineales y angulares, fuerzas externas, etc.
  • Geometrías: Determinan la forma del cuerpo. Se utilizan para la detección de colisiones y la representación en la pantalla. Un cuerpo puede tener la forma de hexaedro, cilincro, espera, plano, etc.

Los objetos a simular en ODE tiene estos dos componentes: el cuerpo y la geometría. Si no estamos interesados en las colisiones ni en la representación gráfica de los objetos, no será necesario definir ninguna geometro (En el ejemplo de la simulación del cubo en caída libre, del capitulo anterior, no definimos ninguna geometría)

Para la simulación de los objetos utilizaremos la estructura MyBody, que contiene los identificadores del cuerpo y la geometría:

struct MyBody {
  dBodyID body;       //-- The ODE body
  dGeomID geom;       //-- The body's geometries
};

Puntos y articulaciones de contacto

Figura 1. Pinchar en la imagen para ampliarla

En la figura 1 se muestran tres instantes de la simulación de un hexaedro en caida libre. En la izquierda el objeto está cayendo y se acerca al suelo. En el instante <math>t_{1}</math> se produce la colisión y aparece un punto de contacto entre la geometría del objeto y el suelo. Durante la transición entre los instantes <math>t_{1}</math> y <math>t_{2}</math>, la trayectoria del objeto se modifica debido a la colisión. En ODE eso se modela colocando una articulación especial sobre el punto de contacto, denominada articulación de contacto. Esta articulación impone restricciones a la hora de moverse el objeto y se tendrá en cuenta al realizar el paso de simulación entre <math>t_{1}</math> y <math>t_{2}</math>.

En el siguiente paso de simulación (instante <math>t_{3}</math>) se repetirá el proceso: se determinará cuáles son los puntos de contactos y se situará sobre ellos una articulación de contacto. Por tanto, los pasos generales a realizar para simular colisiones son:

  1. Determinar si ha habido una colisión y obtener los puntos de contacto
  2. Colocar una articulación de contacto sobre cada uno de ellos
  3. Realizar un paso de simulación
  4. Eliminar las articulaciones de contacto

Los puntos de contacto son del tipo dContact. El número máximo de puntos de contacto permitidos en cada colisión está dado por la consntate MAX_CONTACTS. Los puntos se definen así:

dContact contact[MAX_CONTACTS]; 

Las articulaciones de contacto son del tipo dJointID y se almacenan dentro de un grupo que se define de la forma:

dJointGroupID contactgroup;

Espacios de colisión

La detección de las colisiones sólo se realiza entre las geometrías que están situadas dentro del mismo espacio. Esto permite mejorar el rendimiento de la simulación. Por ejemplo situando los objetos en espacios diferentes.

En el ejemplo box2, se define el siguiente espacio de colisión:

dSpaceID space;

Tanto el plano que hace de suelo como la geometría del cube se deben insertar en este espacio para poder detectar las coliciones entre ellos.

Parámetros del ODE

Explicación del código

Sólo se explica el código nuevo que aparece en relación al ejemplo anterior: Simulación de un cubo en caida libre (I)

Función Main

Una vez creado el mundo virtual, se especifican los valores de ciertas parámetros del ODE:

 dWorldSetCFM (world,CFM);
 dWorldSetContactMaxCorrectingVel (world,MAX_CORRECTING_VEL);
 dWorldSetContactSurfaceLayer (world,SURFACE_LAYER);

Se puede encontrar más información sobre ellas en el manual de ODE:

La bandera de auto-desconexión de los objetos se activa. Sirva para que los objetos que están en reposo se desactiven y no consuman recursos durante la simulación.

 dWorldSetAutoDisableFlag (world,1);

Crear el espacio de colisiones y el grupo para las articulaciones de contacto:

 space = dHashSpaceCreate (0);
 contactgroup = dJointGroupCreate (0);

Finalmente, antes de comenzar la simulación, se crean los objetos a simular: el suelo y el cubo:

 dCreatePlane (space,0,0,1,0);
 Body_new(&box,world,space);

Ambos objetos se incluyen dentro del espacio space para que se puedan detectar las colisiones entre ellos. El suelo es un plano situado en z=0. Los planos se defininen mediante la función dCreatePlane() pasando como argumentos el espacio de colisiones y los coeficientes a,b,c,d de la ecuación del plano: <math>ax + by + cz = d</math>. Los tres primeros coeficientes <math>(a,b,c)</math> determinan el vector normal al plano. En este ejemplo es un vector en dirección del eje z: (0,0,1).

En el bucle principal de la simulación se invoca a la función simloop() que realiza los siguiente:

dSpaceCollide (space,0,&nearCallback);
dWorldStep(world,STEP);
dJointGroupEmpty (contactgroup);

Primero se llama a la función dSpaceCollide() para comprar la colisiones dentro del espacio de colisiones. Si es así, se invoca a la función de retrollamda nearCallback(). Como luego veremos, en esta función es donde se añaden las articulaciones de contacto. A continuación se realiza un paso de simulación. La diferencia con el ejemplo en el que no había colisiones es que ahora pueden haberse colocado las articulaciones de contacto que van a alterar la trayectoria del cubo, impidiendo que siga cayendo. Finalmente se eliminan todas estas articulaciones.

Función nearCallback()

Esta función es invoca por dSpaceCollide() cuando se ha producido una colisión o está muy cercana.

Primero se obtienen los dos cuerpos involucrados en la colisión. Si están conectados por una articulación entonces no se procesa la colisión. Esto lo hacemos así para poder implementar en sucesivos ejemplos los módulos de los robots a simular. Los supondremos compuestos por dos hexaedros unidos mediante una articulación y al rotar no queremos que existan colisiones, sino que una parte pueda penetrar en la otra.

dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) { return; }

A continuación se crean los puntos de contacto y se establecen sus propiedades. Más información sobre los puntos de contacto y sus propiedades

dContact contact[MAX_CONTACTS]; 
  for (i=0; i<MAX_CONTACTS; i++) { 
    contact[i].surface.mode = dContactBounce | dContactSoftCFM;
    contact[i].surface.mu = MU;
    contact[i].surface.mu2 = MU2;
    contact[i].surface.bounce = BOUNCE;
    contact[i].surface.bounce_vel = BOUNCE_VEL;
    contact[i].surface.soft_cfm = SOFT_CFM;
  }

Ahora se comprueban las colisiones y se obtiene la información sobre los puntos de contacto:

 int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, sizeof(dContact));

Para cada punto de contacto se crea una articulación, que une los dos cuerpos que entran en colisión:

 //-- If there are at least one contact point...
 if (numc!=0) {
   
   //-- For every contact point a joint should be created
   for (i=0; i<numc; i++) {
//-- Create the joint and add it to the contact group dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
//-- Set the articulation between the two bodies dJointAttach (c,b1,b2); } }


Icono ok.png Esta función de detección de colisiones es genérica y nos servirá para el resto de ejemplos de este tutorial

Función Body_new: Creación del cubo

Esta es la función encargada de crear el cubo, con su cuerpo y su geometría.

Además de crear el cuerpo, posicionarlo a una cierta altura del suelo y establecer su masa, como en el ejemplo anterior, se crea su geometría y se asocia al cuerpo:

box->geom = dCreateBox (space, W, L, H); 
dGeomSetBody (box->geom,box->body);

Al crear la geometría con la función dCreateBox() se pasa como primer argumento el espacio de colisiones donde situarlo. Tiene que ser el mismo que el del suelo.

Enlaces

Capítulo anterior
Índice
Capítulo siguiente