Diferencia entre revisiones de «Tutorial:ODE y robots modulares:Módulo»

De WikiRobotics
Saltar a: navegación, buscar
(→‎<font color="#0000FF">Simulación de un módulo</font>)
(→‎Vídeo)
 
(No se muestran 23 ediciones intermedias del mismo usuario)
Línea 1: Línea 1:
−
[[Imagen:Servo screenshot3.png|thumb|250px|]]
+
[[Imagen:Servo screenshot3.png|thumb|300px|]]
 
= <font color="#0000FF">Simulación de un módulo</font> =
 
= <font color="#0000FF">Simulación de un módulo</font> =
 
{| {{tablabonita}}
 
{| {{tablabonita}}
Línea 37: Línea 37:
 
  g++  -Iinclude -c -o servo_ex/servo.o servo_ex/servo.cpp
 
  g++  -Iinclude -c -o servo_ex/servo.o servo_ex/servo.cpp
 
  g++  -Iinclude -c -o servo_ex/module.o servo_ex/module.cpp
 
  g++  -Iinclude -c -o servo_ex/module.o servo_ex/module.cpp
−
  g++ -o servo servo_ex/servo.o servo_ex/module.o  libdrawstuff.a -lm -lode  -lX11 -lm -lGL -lGLU
+
  g++ -o servo servo_ex/servo.o servo_ex/module.o  libdrawstuff.a -lm -lode  -lX11 -lGL -lGLU
  
 
== Ejecución ==
 
== Ejecución ==
Línea 68: Línea 68:
 
{| {{tablabonita}}
 
{| {{tablabonita}}
 
| [[Imagen:Servo video thumb.png]]
 
| [[Imagen:Servo video thumb.png]]
−
| [http://www.iearobotics.com/wiki/images/b/b7/Tutorial_ode_servo_video.mpg video.mpg]
+
|  
 +
* [http://www.iearobotics.com/wiki/images/b/b7/Tutorial_ode_servo_video.mpg video.mpg]
 +
* [http://www.youtube.com/watch?v=KRviim78_wo Vídeo en Youtube]
 
|}
 
|}
  
 
== Modelo de módulo ==
 
== Modelo de módulo ==
 +
 +
[[Imagen:Module.png|thumb|350px|'''Figura 1''': El módulo empleado en este ejemplo (pinchar para ampliar)]]
 +
 +
 +
El módulo está formado por dos cuerpos, que denominaremos izquierdo y derecho. Tienen diferente masa y longitud, pero la altura y anchura son las mismas. Ambos cuerpos tienen como geometría un hexaedro, como se muestra en la '''figura 1'''.
 +
* '''Cuerpo izquierdo''':
 +
** '''Masa''': MASS0
 +
** '''Dimensiones''': W x L0 x H
 +
* '''Cuerpo derecho''':
 +
** '''Masa''': MASS1
 +
** '''Dimensiones''': W x L1 x H
 +
La '''estructura de datos''' para representar el módulo está definida en el fichero '''module.h''':
 +
 +
struct MyModule {
 +
  dJointID joint;     
 +
  dReal servo_ref_pos; 
 +
 
 +
  dBodyID body_left;   
 +
  dGeomID geom_left;     
 +
 
 +
  dBodyID body_right;   
 +
  dGeomID geom_right;
 +
};
 +
 +
Además de los dos cuerpos y sus correspondientes geometrías, la estructura tiene el identificador de la articulación y la posición angular en la que situar el servo: '''servo_ref_pos'''.
 +
 +
Los dos hexaedros que forman los cuerpos unidos por una articulación '''rotan alrededor del eje z''' que pasa por '''el punto de anclaje de la articulación''', situado en las coordenadas (0,L0,0) con respecto al origen de coordenadas.
 +
 +
Al rotar, los dos hexaedros entran en colisión. Sin embargo, como vimos en el [[Tutorial:ODE_y_robots_modulares:Caida_libre_(II)|ejemplo del capítulo 2]], si dos objetos están conectados mediante una articulación, las colisiones no se tienen en cuenta. Por ello, los dos hexaedros pueden rotar y penetrarán el uno en el otro.
  
 
== Funcionamiento del servo ==
 
== Funcionamiento del servo ==
 +
 +
{| {{tablabonita}}
 +
| [[Imagen:Tutorial ode module servo pos.png]] <br>
 +
'''Figura 2''': Respuesta del servo al escalón
 +
| [[Imagen:Tutorial ode module servo vel.png]] <br>
 +
'''Figura 3''': Velocidad del servo al aplicar un escalón
 +
|}
 +
 +
Para la implementación del servo se han seguido las recomendaciones indicadas en el manual del ODE. Se ha utilizado '''un controlador de tipo proporcional''' que viene dado por la ecuación: <math> w\left(t\right)=K_{p}\left[\varphi\left(t\right)-p\right]</math>, donde:
 +
 +
{| {{tablabonita}}
 +
|
 +
* <math>w\left(t\right)</math>: es la velocidad angular instantánea
 +
|
 +
* <math>K_{p}</math>: Es la ganancia del servo
 +
|----------
 +
|
 +
* <math>\varphi\left(t\right)</math>: Posición instantánea del servo
 +
|
 +
* '''p''': Es la posición de referencia
 +
|}
 +
En la '''figura 2''' se muestra la respuesta al escalón. En el instante t=200 se establece como posición de referencia p=90 grados. el servo se encuentra en la posición de 0 grados. La posición del servo empieza a aumentar. Según se acerca a la posición de referencia su velocidad va disminuyendo hasta llegar a 0. En la '''figura 3''' se muestra la velocidad angular para ese mismo ejemplo. La velocidad máxima está limitada por la que tiene el servo (WMAX) por lo que hay saturación. A partir de un instante, esta velocidad comienza a disminuir hasta llegar a 0.
  
 
== Explicación del código ==
 
== Explicación del código ==
 +
=== Función Module_new() ===
 +
Esta es la función que se encarga de crear el módulo entero. Primero se crea el '''cuerpo izquierdo''' y se establece la posición de su centro de masas. Tomando como referencia el origen indicado anteriormente, se debe situar en las coordenadas (0,-L0/2,H/2):
 +
 +
  mod->body_left = dBodyCreate(world);
 +
  dBodySetPosition(mod->body_left, 0.0, -L0/2, H/2);
 +
 
 +
Luego se le asigna la masa (MASS0) y se establecen las dimensiones del hexaedro que delimita el cuerpo, que es uno de dimensiones W x L0 x H:
 +
 +
  dMassSetBoxTotal (&m, MASS0, W, L0, H);
 +
  dBodySetMass (mod->body_left,&m);
 +
 
 +
Ahora se crea la geometría, que es un hexaedro de las mismas dimensiones anteriores y se asigna al cuerpo izquierdo:
 +
 +
  mod->geom_left = dCreateBox (space, W, L0, H);
 +
  dGeomSetBody (mod->geom_left,mod->body_left);
 +
 +
Lo siguiente es crear el '''cuerpo derecho'''. El proceso es el mismo pero ahora su centro de masas se encuentra en la posición (0,-L0-L1/2, H/2), sus dimensiones son (W,L1,H) y su masas MASS1:
 +
 +
  mod->body_right = dBodyCreate(world);
 +
  dBodySetPosition(mod->body_right, 0.0, -L0-L1/2, H/2);
 +
  dMassSetBoxTotal (&m, MASS1, W, L1,H);
 +
  dBodySetMass (mod->body_right,&m);
 +
  mod->geom_right = dCreateBox (space, W, L1, H);
 +
  dGeomSetBody (mod->geom_right,mod->body_right);
 +
 +
Por último '''se crea la articulación'''. Se sitúa en el punto (0,-L0,H/2), se asigna el eje z como eje de rotación (0,0,1) y se establece su fuerza máxima, dada por la constante TORQUE y una velocidad angular inicial de 0:
 +
 +
  mod->joint=dJointCreateHinge (world, 0);
 +
  dJointAttach (mod->joint,mod->body_left,mod->body_right);
 +
  dJointSetHingeAnchor (mod->joint, 0, -L0, H/2);
 +
  dJointSetHingeAxis (mod->joint, 0,0,1);
 +
  dJointSetHingeParam(mod->joint, dParamFMax, TORQUE);
 +
  dJointSetHingeParam(mod->joint, dParamVel, 0.0);
 +
 +
=== Función Module_render() ===
 +
Esta es la función encargada de '''dibujar el módulo'''. Es muy sencilla ya que el módulo sólo tiene dos geometrías. Es similar a los ejemplos anteriores: Primero se establece la textura y luego se dibuja la geometría del cuerpo izquierdo con un color y la del derecho con otro:
 +
 +
  dsSetTexture (DS_WOOD);
 +
  dsSetColor (0.9,0,0);
 +
  drawGeom(mod->geom_left);
 +
  dsSetColor (1,0,0);
 +
  drawGeom (mod->geom_right);
 +
 +
=== Simulación del servo ===
 +
En el bucle principal de simulación (función '''simLoop''') es igual que en los ejemplos anteriores, pero ahora además para cada tic de simulación hay que recalcular la velocidad angular del servo (w(t)) que viene dada por la diferencia entre su posición actual (<math>\varphi </math>) y la posición de referencia (ver ecuación anterior).
 +
 +
Después de realizar el paso de simulación, se invoca a la función '''servos_sim()''' para realizar estos cálculos:
 +
 +
  ...
 +
  dWorldStep(world,STEP);
 +
  servos_sim();
 +
  ...
 +
 +
En esta función primero se obtiene la posición actual de la articulación, llamando a la función '''dJointGetHingeAngle()''':
 +
 +
  dReal pos = dJointGetHingeAngle(mod.joint);
 +
 +
A continuación se calcula la diferencia entre esta posición y la de referencia, que se encuentra en el campo '''servo_ref_pos'''. Esta diferencia se denomina error. Es la distancia angular que le queda por recorrer al servo para alcanzar su posición objetivo: (<font color="#FF0000">¡Se calcula en radianes!</font> )
 +
 +
  dReal error = pos - DEG2RAD(mod.servo_ref_pos);
 +
 
 +
La velocidad a aplicar al servo es proporcional a este error. Además hay que limitarla para que nunca sea superior a la máxima permitida (WMAX):
 +
 +
  dReal velocity = -error*KP;
 +
  if (velocity > WMAX) velocity = WMAX;
 +
  if (velocity < -WMAX) velocity = -WMAX;
 +
 +
Por último se establece la velocidad del servo para ese intante: 
 +
 +
  dJointSetHingeParam(mod.joint, dParamVel, velocity);
 +
 +
=== Funciones Main y command===
 +
La función principal es similar a la empleada en [[Tutorial:ODE y robots modulares:Cuerpos compuestos (II)|en el ejemplo del capítulo anterior]] pero ahora para crear el módulo se invoca a la función Module_New().
 +
 +
La función '''command()''' es la que se ejecuta cuando el usuario presiona una tecla. Según que se haya pulsado '1', '2' ó '3', se dan los valores de 90, 0 y -90 respectivamente a la posición de referencia del servo (servo_ref_pos) de manera que en el siguiente tic de simulación se volverá a recalcular el error (diferencia entre la posición del servo y la de referencia) y se asignará una nueva velocidad al servo, con lo que se moverá hacia la nueva posición.
 +
 +
if (cmd=='1') {       
 +
  mod.servo_ref_pos=90;
 +
}
 +
else if (cmd=='2') { 
 +
  mod.servo_ref_pos=0;
 +
}
 +
else if (cmd=='3') { 
 +
  mod.servo_ref_pos=-90;
 +
}
 +
else if (cmd=='q') {
 +
  //-- Finish the simulation and exit
 +
  dsStop();
 +
}
 +
 +
Lo interesante es que a este nivel '''para mover el servo sólo hay que colocar la posición deseada en una variable'''. El bucle principal de la simulación se encarga del resto. '''Así logramos un interfaz igual que con los servos reales'''. Al software situado a este nivel, no sabe si lo que mueve es un servo real o uno simulado, simplemente establece la posición donde se debe situar el servo.
 +
 
== Enlaces ==
 
== Enlaces ==
 
{| {{tablabonita}}
 
{| {{tablabonita}}

Revisión actual del 02:03 13 ene 2009

Servo screenshot3.png

Simulación de un módulo

Capítulo anterior
Índice
Capítulo siguiente

Introducción

El siguiente paso para empezar a simular robots modulares es saber cómo implementar un servo. Construiremos un módulo constituido por dos cuerpos unidos mediante un servo. En la simulación de ejemplo, el usuario podrá establecer las posiciones de -90, 90 y 0 grados para este servo y verá cómo varían las posiciones relativas de los dos cuerpos.

Objetivo

  • Aprender a simular un servo y construir nuestro primer módulo

Código

Programa principal
Creación y dibujo del módulo
Definición de las constantes.
Definición de los prototipos de modulo.cpp y las estructuras de datos.

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 -c -o servo_ex/servo.o servo_ex/servo.cpp
g++  -Iinclude -c -o servo_ex/module.o servo_ex/module.cpp
g++ -o servo servo_ex/servo.o servo_ex/module.o  libdrawstuff.a -lm -lode  -lX11 -lGL -lGLU

Ejecución

Para probar el ejemplo, teclear: 

./servo

Además de los mensajes impresos en pantalla por la drawstuff, aparecerá el siguiente menú: 

 Keys for moving the servo: 
 1: Set the servo angle to 90 degrees
 2: Set the servo angle to  0 degrees
 3: Set the servo angle to -90 degrees
 q: Quit

Por medio de las teclas '1', '2' y '3' el usuario puede establecer la posición del servo a 90, 0 y -90 grados respectivamente.

Capturas de pantalla

Visualización del módulo en las tres posiciones:

Servo en posición de 90 grados
Servo en posición de 0 grados
Servo en posición de -90 grados

Vídeo

Servo video thumb.png

Modelo de módulo

Figura 1: El módulo empleado en este ejemplo (pinchar para ampliar)


El módulo está formado por dos cuerpos, que denominaremos izquierdo y derecho. Tienen diferente masa y longitud, pero la altura y anchura son las mismas. Ambos cuerpos tienen como geometría un hexaedro, como se muestra en la figura 1.

  • Cuerpo izquierdo:
    • Masa: MASS0
    • Dimensiones: W x L0 x H
  • Cuerpo derecho:
    • Masa: MASS1
    • Dimensiones: W x L1 x H

La estructura de datos para representar el módulo está definida en el fichero module.h: 

struct MyModule {
  dJointID joint;       
  dReal servo_ref_pos;   
 
  dBodyID body_left;     
  dGeomID geom_left;       
 
  dBodyID body_right;     
  dGeomID geom_right;
};

Además de los dos cuerpos y sus correspondientes geometrías, la estructura tiene el identificador de la articulación y la posición angular en la que situar el servo: servo_ref_pos.

Los dos hexaedros que forman los cuerpos unidos por una articulación rotan alrededor del eje z que pasa por el punto de anclaje de la articulación, situado en las coordenadas (0,L0,0) con respecto al origen de coordenadas.

Al rotar, los dos hexaedros entran en colisión. Sin embargo, como vimos en el ejemplo del capítulo 2, si dos objetos están conectados mediante una articulación, las colisiones no se tienen en cuenta. Por ello, los dos hexaedros pueden rotar y penetrarán el uno en el otro.

Funcionamiento del servo

Tutorial ode module servo pos.png

Figura 2: Respuesta del servo al escalón

Tutorial ode module servo vel.png

Figura 3: Velocidad del servo al aplicar un escalón

Para la implementación del servo se han seguido las recomendaciones indicadas en el manual del ODE. Se ha utilizado un controlador de tipo proporcional que viene dado por la ecuación: <math> w\left(t\right)=K_{p}\left[\varphi\left(t\right)-p\right]</math>, donde:

  • <math>w\left(t\right)</math>: es la velocidad angular instantánea
  • <math>K_{p}</math>: Es la ganancia del servo
  • <math>\varphi\left(t\right)</math>: Posición instantánea del servo
  • p: Es la posición de referencia

En la figura 2 se muestra la respuesta al escalón. En el instante t=200 se establece como posición de referencia p=90 grados. el servo se encuentra en la posición de 0 grados. La posición del servo empieza a aumentar. Según se acerca a la posición de referencia su velocidad va disminuyendo hasta llegar a 0. En la figura 3 se muestra la velocidad angular para ese mismo ejemplo. La velocidad máxima está limitada por la que tiene el servo (WMAX) por lo que hay saturación. A partir de un instante, esta velocidad comienza a disminuir hasta llegar a 0.

Explicación del código

Función Module_new()

Esta es la función que se encarga de crear el módulo entero. Primero se crea el cuerpo izquierdo y se establece la posición de su centro de masas. Tomando como referencia el origen indicado anteriormente, se debe situar en las coordenadas (0,-L0/2,H/2): 

 mod->body_left = dBodyCreate(world);
 dBodySetPosition(mod->body_left, 0.0, -L0/2, H/2);

Luego se le asigna la masa (MASS0) y se establecen las dimensiones del hexaedro que delimita el cuerpo, que es uno de dimensiones W x L0 x H: 

 dMassSetBoxTotal (&m, MASS0, W, L0, H);
 dBodySetMass (mod->body_left,&m);

Ahora se crea la geometría, que es un hexaedro de las mismas dimensiones anteriores y se asigna al cuerpo izquierdo: 

 mod->geom_left = dCreateBox (space, W, L0, H); 
 dGeomSetBody (mod->geom_left,mod->body_left);

Lo siguiente es crear el cuerpo derecho. El proceso es el mismo pero ahora su centro de masas se encuentra en la posición (0,-L0-L1/2, H/2), sus dimensiones son (W,L1,H) y su masas MASS1: 

 mod->body_right = dBodyCreate(world);
 dBodySetPosition(mod->body_right, 0.0, -L0-L1/2, H/2);
 dMassSetBoxTotal (&m, MASS1, W, L1,H);
 dBodySetMass (mod->body_right,&m);
 mod->geom_right = dCreateBox (space, W, L1, H); 
 dGeomSetBody (mod->geom_right,mod->body_right);

Por último se crea la articulación. Se sitúa en el punto (0,-L0,H/2), se asigna el eje z como eje de rotación (0,0,1) y se establece su fuerza máxima, dada por la constante TORQUE y una velocidad angular inicial de 0: 

 mod->joint=dJointCreateHinge (world, 0);
 dJointAttach (mod->joint,mod->body_left,mod->body_right);
 dJointSetHingeAnchor (mod->joint, 0, -L0, H/2);
 dJointSetHingeAxis (mod->joint, 0,0,1); 
 dJointSetHingeParam(mod->joint, dParamFMax, TORQUE);
 dJointSetHingeParam(mod->joint, dParamVel, 0.0);

Función Module_render()

Esta es la función encargada de dibujar el módulo. Es muy sencilla ya que el módulo sólo tiene dos geometrías. Es similar a los ejemplos anteriores: Primero se establece la textura y luego se dibuja la geometría del cuerpo izquierdo con un color y la del derecho con otro: 

 dsSetTexture (DS_WOOD);
 dsSetColor (0.9,0,0);
 drawGeom(mod->geom_left);
 dsSetColor (1,0,0);
 drawGeom (mod->geom_right);

Simulación del servo

En el bucle principal de simulación (función simLoop) es igual que en los ejemplos anteriores, pero ahora además para cada tic de simulación hay que recalcular la velocidad angular del servo (w(t)) que viene dada por la diferencia entre su posición actual (<math>\varphi </math>) y la posición de referencia (ver ecuación anterior).

Después de realizar el paso de simulación, se invoca a la función servos_sim() para realizar estos cálculos: 

 ...
 dWorldStep(world,STEP);
 servos_sim();
 ...

En esta función primero se obtiene la posición actual de la articulación, llamando a la función dJointGetHingeAngle(): 

 dReal pos = dJointGetHingeAngle(mod.joint);

A continuación se calcula la diferencia entre esta posición y la de referencia, que se encuentra en el campo servo_ref_pos. Esta diferencia se denomina error. Es la distancia angular que le queda por recorrer al servo para alcanzar su posición objetivo: (¡Se calcula en radianes! ) 

 dReal error = pos - DEG2RAD(mod.servo_ref_pos);

La velocidad a aplicar al servo es proporcional a este error. Además hay que limitarla para que nunca sea superior a la máxima permitida (WMAX): 

 dReal velocity = -error*KP;
 if (velocity > WMAX) velocity = WMAX;
 if (velocity < -WMAX) velocity = -WMAX;

Por último se establece la velocidad del servo para ese intante: 

 dJointSetHingeParam(mod.joint, dParamVel, velocity);

Funciones Main y command

La función principal es similar a la empleada en en el ejemplo del capítulo anterior pero ahora para crear el módulo se invoca a la función Module_New().

La función command() es la que se ejecuta cuando el usuario presiona una tecla. Según que se haya pulsado '1', '2' ó '3', se dan los valores de 90, 0 y -90 respectivamente a la posición de referencia del servo (servo_ref_pos) de manera que en el siguiente tic de simulación se volverá a recalcular el error (diferencia entre la posición del servo y la de referencia) y se asignará una nueva velocidad al servo, con lo que se moverá hacia la nueva posición. 

if (cmd=='1') {        
  mod.servo_ref_pos=90;
}
else if (cmd=='2') {   
  mod.servo_ref_pos=0;
}
else if (cmd=='3') {   
  mod.servo_ref_pos=-90;
}
else if (cmd=='q') {
  //-- Finish the simulation and exit
  dsStop();
}

Lo interesante es que a este nivel para mover el servo sólo hay que colocar la posición deseada en una variable. El bucle principal de la simulación se encarga del resto. Así logramos un interfaz igual que con los servos reales. Al software situado a este nivel, no sabe si lo que mueve es un servo real o uno simulado, simplemente establece la posición donde se debe situar el servo.

Enlaces

Capítulo anterior
Índice
Capítulo siguiente
Obtenido de «http://iearobotics.com/wiki/index.php?title=Tutorial:ODE_y_robots_modulares:Módulo&oldid=4677»