Diferencia entre revisiones de «Tutorial:ODE y robots modulares:Robot ápodo»
(→Función Robot_new()) |
(→Vídeo) |
||
(No se muestran 9 ediciones intermedias del mismo usuario) | |||
Línea 73: | Línea 73: | ||
{| {{tablabonita}} | {| {{tablabonita}} | ||
| [[Imagen:Snake video thumb.png]] | | [[Imagen:Snake video thumb.png]] | ||
− | | [http://www.iearobotics.com/wiki/images/a/af/Tutorial_ode_snake_video.mpg video.mpg] | + | | |
+ | * [http://www.iearobotics.com/wiki/images/a/af/Tutorial_ode_snake_video.mpg video.mpg] | ||
+ | * [http://www.youtube.com/watch?v=tlsfUvzfJB0 Vídeo en Youtube] | ||
|} | |} | ||
Línea 269: | Línea 271: | ||
== Explicación del código == | == Explicación del código == | ||
=== Función Robot_new() === | === Función Robot_new() === | ||
− | Es la función encargada de construir el modelo ODE de la serpiente. Se divide en varias partes: | + | Es la función encargada de '''construir el modelo ODE de la serpiente'''. Construye robots del grupo cabeceo-viraje, de número de módulos MOD, con MOD>=1. Se divide en varias partes: |
Primero se construye el '''cuerpo izquierdo''', con su geometría asociada: | Primero se construye el '''cuerpo izquierdo''', con su geometría asociada: | ||
Línea 361: | Línea 363: | ||
=== Función Robot_Render() === | === Función Robot_Render() === | ||
+ | Es la función que se encarga de dibujar la serpiente en pantalla. Hay que representar las 2*MOD geometrías que tiene el robot. Dependiendo de que la geometría pertenezca a un módulo par o impar, el color utilizado será amarillo o rojo. | ||
+ | |||
+ | Primero se dibuja la '''geomtría izquierda''', asociada al cuerpo izquierdo, en color amarillo: | ||
+ | |||
+ | dsSetColor (1,1,0); | ||
+ | drawGeom (snake->geom_left); | ||
+ | |||
+ | A continuación se dibujan todas las '''geomtrías de los cuerpos centrales''', cambiando el color: | ||
+ | |||
+ | for (int i=0; i<MOD-1; i++) { | ||
+ | if (i%2==0) G=1; | ||
+ | else G=0; | ||
+ | |||
+ | //-- Draw the first box | ||
+ | dsSetColor (1,G,0); | ||
+ | drawGeom(snake->geom1[i]); | ||
+ | |||
+ | //-- Draw the second box | ||
+ | dsSetColor (1,!G,0); | ||
+ | drawGeom(snake->geom2[i]); | ||
+ | } | ||
+ | |||
+ | Por último se dibuja la '''geometría derecha'''. Su color será diferente al de la geometría anterior: | ||
+ | |||
+ | dsSetColor (1,!G,0); | ||
+ | drawGeom (snake->geom_right); | ||
+ | |||
=== Función servos_sim() === | === Función servos_sim() === | ||
+ | |||
+ | Esta función es igual que la que se implementó en el [[Tutorial:ODE y robots modulares:Configuración PP|capítulo anterior]], pero ahora en vez de dos servos hay que simular MOD. Por ello, sólo cambia la definición del bucle: | ||
+ | |||
+ | for (int i=0; i<MOD; i++) { | ||
+ | |||
+ | El resto del código es igual | ||
+ | |||
=== Función sequence_generation() === | === Función sequence_generation() === | ||
− | === Funciones | + | Esta función se encarga de '''generar las posiciones de referencia para todos los servos'''. Las ecuaciones de los osciladores son diferentes dependiendo de si son verticales (módulos impares) u horizontales (módulos pares): |
+ | |||
+ | for (int i=0; i<MOD; i++) { | ||
+ | if (i%2==0) { | ||
+ | phase = PDv*(i/2) + F0; | ||
+ | snake.servo_ref_pos[i]=Av*sin(2*M_PI*n/N + DEG2RAD(phase) ); | ||
+ | } | ||
+ | else { | ||
+ | phase = PDh*(i-1)/2 + PDvh + F0; | ||
+ | snake.servo_ref_pos[i]=Ah*sin(2*M_PI*n/N + DEG2RAD(phase))+Oh; | ||
+ | } | ||
+ | } | ||
+ | n = (n + 1) % N; | ||
+ | |||
+ | === Funciones para implementar la locomoción === | ||
+ | Para mover el robot hay que especificar el valor de los diferentes parámetros del espacio de formas y calcular a partir de ellos los parámetros de los osciladores del espacio de control. Dependiendo del modo de caminar que se quiere lograr, las funciones para ello son: | ||
+ | |||
+ | * '''Movimiento en línea recta''': | ||
+ | |||
+ | void straight(dReal alfa,dReal k) | ||
+ | { | ||
+ | Ah=0; Oh=0; F0=0; | ||
+ | Av=2*alfa*sin(2*M_PI*k/MOD); | ||
+ | PDv=direction*(4*M_PI*k/MOD)*180/M_PI; | ||
+ | } | ||
+ | |||
+ | * '''Giros''': | ||
+ | |||
+ | void turning(dReal alfah, dReal kv) | ||
+ | { | ||
+ | Ah=0; F0=0; | ||
+ | Oh=2*alfah/MOD; | ||
+ | Av=2*20*sin(2*M_PI*kv/MOD); | ||
+ | PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; | ||
+ | } | ||
+ | |||
+ | * '''Desplazamiento lateral''': | ||
+ | |||
+ | void side_winding(dReal alfah, dReal kh) | ||
+ | { | ||
+ | dReal kv=kh; | ||
+ | |||
+ | Av=2*5*sin(2*M_PI*kv/MOD); | ||
+ | PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; | ||
+ | Oh=0; | ||
+ | Ah=2*alfah*sin(2*M_PI*kh/MOD); | ||
+ | PDh=PDv; | ||
+ | PDvh=0; | ||
+ | } | ||
+ | |||
+ | * '''Desplazamiento lateral inclinado''': | ||
+ | |||
+ | void inclined_side_winding(dReal alfa, dReal k) | ||
+ | { | ||
+ | dReal alfav = alfa*sin(45); | ||
+ | dReal alfah = alfav; | ||
+ | dReal kv=k; | ||
+ | dReal kh=k; | ||
+ | |||
+ | Av=2*alfav*sin(2*M_PI*kv/MOD); | ||
+ | PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; | ||
+ | Oh=0; | ||
+ | Ah=2*alfah*sin(2*M_PI*kh/MOD); | ||
+ | PDh=PDv; | ||
+ | PDvh=0; | ||
+ | } | ||
+ | |||
+ | * '''Rotación en S''': | ||
+ | |||
+ | void rotating_S(dReal alfah, dReal kh) | ||
+ | { | ||
+ | dReal kv=2*kh; | ||
+ | Av=2*5*sin(2*M_PI*kv/MOD); | ||
+ | PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; | ||
+ | |||
+ | Oh=0; | ||
+ | Ah=2*alfah*sin(2*M_PI*kh/MOD); | ||
+ | PDh=direction*(4*M_PI*kh/MOD)*180/M_PI; | ||
+ | PDvh=0; | ||
+ | } | ||
+ | |||
+ | * '''Rotación en U''': | ||
+ | void rotating_U(dReal alfa) | ||
+ | { | ||
+ | dReal kv=2; | ||
+ | |||
+ | Av=2*5*sin(2*M_PI*kv/MOD); | ||
+ | PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; | ||
+ | Oh=0; | ||
+ | Ah=2*alfa/MOD; | ||
+ | PDh=0; | ||
+ | PDvh=direction*90; | ||
+ | } | ||
+ | |||
+ | * '''Remero''': | ||
+ | void flapping() | ||
+ | { | ||
+ | dReal alfa=12; | ||
+ | |||
+ | Av=2*alfa/MOD; | ||
+ | Ah=Av; PDv=0; PDh=0; | ||
+ | PDvh=direction*90; | ||
+ | Oh=0; | ||
+ | } | ||
+ | |||
+ | * '''Rodar''': | ||
+ | void rotating_S(dReal alfah, dReal kh) | ||
+ | { | ||
+ | dReal kv=2*kh; | ||
+ | |||
+ | Av=2*5*sin(2*M_PI*kv/MOD); | ||
+ | PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; | ||
+ | Oh=0; | ||
+ | Ah=2*alfah*sin(2*M_PI*kh/MOD); | ||
+ | PDh=direction*(4*M_PI*kh/MOD)*180/M_PI; | ||
+ | PDvh=0; | ||
+ | } | ||
+ | |||
+ | === Función Command() === | ||
+ | Esta es la '''función que selecciona el modo de caminar según la tecla pulsada''' por el usuario. Cada vez que se pulsa una tecla, el sentido del movimiento del robot se cambia. Se invoca a las funciones definidas en el apartado anterior: | ||
+ | |||
+ | if (cmd=='1') { | ||
+ | direction=direction*-1; | ||
+ | straight(20,2); | ||
+ | } | ||
+ | else if (cmd=='2') { | ||
+ | direction=direction*-1; | ||
+ | turning(180,2); | ||
+ | } | ||
+ | else if (cmd=='3') { | ||
+ | direction=direction*-1; | ||
+ | side_winding(40,1); | ||
+ | } | ||
+ | else if (cmd=='4') { | ||
+ | direction=direction*-1; | ||
+ | inclined_side_winding(40,1); | ||
+ | } | ||
+ | else if (cmd=='5') { | ||
+ | direction=direction*-1; | ||
+ | flapping(); | ||
+ | } | ||
+ | else if (cmd=='6') { | ||
+ | direction=direction*-1; | ||
+ | rotating_S(40,1); | ||
+ | } | ||
+ | else if (cmd=='7') { | ||
+ | direction=direction*-1; | ||
+ | rotating_U(180); | ||
+ | } | ||
+ | else if (cmd=='8') { | ||
+ | direction=direction*-1; | ||
+ | rolling(180); | ||
+ | } | ||
+ | else if (cmd=='q') { | ||
+ | dsStop(); | ||
+ | } | ||
== Enlaces == | == Enlaces == |
Revisión actual del 03:11 13 ene 2009
Contenido
- 1 Simulación de un robot ápodo del grupo cabeceo-viraje
Simulación de un robot ápodo del grupo cabeceo-viraje
|
|
Introducción
En este ejemplo se simula un robot ápodo del grupo cabeceo-viraje de 16 módulos. El usuario puede establecer los diferentes modos de caminar mediante el teclado: línea recta, desplazamiento lateral, rotación, rodar, girar, etc. Para la generación de todos los movimientos se utiliza la misma técnica de generadores sinusoidales que en el ejemplo de la cocnfiguración PP.
Objetivo
- Mostrar cómo mover un robot ápodo de cualquier número de módulos
Código
Programa principal | |
Creación y dibujo del robot | |
Definición de las constantes. | |
Definición de los prototipos de robot.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 snake_ex/snake.o snake_ex/snake.cpp g++ -Iinclude -c -o snake_ex/robot.o snake_ex/robot.cpp g++ -o snake snake_ex/snake.o snake_ex/robot.o libdrawstuff.a -lm -lode -lX11 -lGL -lGLU
Ejecución
Para probar el ejemplo, teclear:
./snake
Además de los mensajes impresos en pantalla por la drawstuff, aparecerá el siguiente menú:
Keys for selecting the gaits: 1: Straight 2: Turning 3: Side-winding 4: Inclined side-winding 5: Flapping 6: S-shaped rotating 7: U-sahped rotating 8: Rolling q: Quit simulation
Mediante las diferentes teclas se pueden seleccionar las 8 maneras de caminar del robot ápodo
Capturas de pantalla
Visualización del robot moviéndose de diferentes maneras:
Vídeo
Clasificación de robots ápodos
Figura 1: Clasificación de los robots ápodos: grupo cabeceo-cabeceo (izquierda), viraje-viraje (centro) y cabeceo-viraje (derecha) |
Los robots ápodos se pueden clasificar en tres grupos dependiendo de la conexión entre sus módulos, como se muestra en la figura 1. Los grupos son: cabeceo-cabeceo, viraje-viraje y cabeceo-viraje.
En este capítulo se simula la locomoción de un robot ápodo del grupo cabeceo-viraje.
Modelo de robot
El robot está constituido por la unión en cadena de MOD módulos, numerados desde el módulo 0 hasta el MOD-1. Los impares son de cabeceo y los pares de viraje. En la Figura 1 se muestra la geometría de los dos primeros módulos del robot, el 0 y el 1.
El robot está formado por:
- MOD módulos iguales, de masa MASS y dimensiones (W,L,H).
- MOD articulaciones, de cuales las impares son de cabeceo y las pares de viraje. Sus ejes de rotación son el x (1,0,0) y el z (0,0,1) respectivamente.
- MOD+1 cuerpos:
- El izquierdo y derecho están formados por una única geometría hexaédrica, de dimensiones (W,L/2,H) y masa MASS/2
- Los cuerpos centrales están compuestos por la unión de dos geometrías hexaédricas. Tienen masa MASS y dimensiones (W,L,H)
- 2*MOD geometrías, de dimensiones (W,L/2,H). Los cuerpos centrales tiene dos geometrías, denominadas 1 y 2,
Las posiciones de los elementos con respecto al origen de coordenadas son:
Cuerpo izquierdo: | (0,-L/4,H/2) |
Cuerpo derecho: | (0, -MOD*L + l/4, H/2) |
Cuerpo central i: | (0, -L(i+1), H/2) |
Articulación i | (0, -iL-L/2, H/2) |
Las dos geometrías de los cuerpo central están desplazadas -L/4 y L/4 a lo largo del eje y.
La estructura de datos está definida en el fichero robot.h:
struct MySnake { dJointID joint[MOD]; //-- Robot joints dBodyID body[MOD-1]; //-- Central bodies //-- Every central body is composed of two boxes dGeomID geom1[MOD-1]; //-- One in the left and another dGeomID geom2[MOD-1]; //-- in the right dBodyID body_left; //-- This is the body on the left of the snake dGeomID geom_left; //-- and its geometry (a box) dBodyID body_right; //-- This is the body on the right of the snake dGeomID geom_right; //-- and its geometry (a box) dReal servo_ref_pos[MOD]; //-- Reference positions for the servos };
Modelo de control
Para el control del robot usaremos la misma idea que para la configuración PP del capítulo anterior: un modelo bioinspirado basado en generadores sinusoidales que hacen oscilar las articulaciones.
Se usan dos grupos de generadores, que denominaremos verticales y horizontales para controlar las articulaciones de cabeceo y viraje respectivamente. El esquema se muestra en la figura 2. Los generadores pares son los verticales y los impares los horizontales.
Las posiciones de referencia de los servos se calculan mediante las siguientes ecuaciones:
- Módulos verticales:
- <math>\varphi_{vi}\left(n\right)=A_{v}\sin\left(\frac{2\pi}{N}n+\frac{i}{2}\Delta\phi_{v}+\phi_{0}\right) </math>, para valores de i pares entre [0,MOD-1]
- Módulos horizontales:
- <math> \varphi_{hi}\left(n\right)=A_{h}\sin\left(\frac{2\pi}{N}n+\frac{\left(i-1\right)}{2}\Delta\phi_{h}+\Delta\phi_{v}+\phi_{0}\right)+o_{h} </math>, para valores de i impares entre [1,MOD-1]
Los parámetros son:
<math>A_{v}</math> | Amplitud de los osciladores verticales (grados) |
<math>A_{h}</math> | Amplitud de los osciladores horizontales (grados) |
<math>\Delta\Phi_{v}</math> | Diferencia de fase entre los generadores verticales (radianes) |
<math>\Delta\Phi_{h}</math> | Diferencia de fase entre los generadores horizontales (radianes) |
<math>\Delta\Phi_{vh}</math> | Diferencia de fase entre los generadores verticales y horizontales (radianes) |
<math>\Phi_{0}</math> | Fase inicial (radianes) |
<math>o_{h}</math> | Offset de los generadores horizontales (grados) |
N | Número de muestras por ciclo |
n | Tiempo discreto |
Espacio de formas
El modo de caminar de los robots ápodos del grupo cabeceo-viraje depende de la forma de la onda tridimensional que recorre su cuerpo. Esta onda es el resultado de la superposición de una que se propaga por las articulaciones horizontales (la onda horizontal) y otra por las verticales (la onda vertical).
Tanto las ondas verticales como horizontales son de tipo serpentinoide, caracterizadas por sus dos parámetros ángulo de serpenteo <math>\alpha</math> y número de ondulaciones k. La forma de la onda tridimensional está determinada por los siguientes parámetros:
<math>\alpha_v</math> | Ángulo de serpenteo de la onda vertical |
<math>k_{v}</math> | Número de ondulaciones de la onda vertical |
<math>\alpha_h</math> | Ángulo de serpenteo de la onda vertical |
<math>k_{h}</math> | Número de ondulaciones de la onda vertical |
<math>\Delta\Phi_{vh}</math> | Diferencia de fase entra la onda vertical y horizontal |
El punto de trabajo en el espacio de formas permite establecer las características de la locomoción. Aplicando las siguientes ecuaciones se obtienen los parámetros en el espacio de control para aplicar a los generadores:
- <math>A_{h}=2\alpha_{h}\sin\left(\frac{2\pi k_{h}}{MOD}\right)</math>
- <math>A_{v}=2\alpha_{V}\sin\left(\frac{2\pi k_{v}}{MOD}\right)</math>
- <math>\Delta\phi_{V}=\frac{4\pi k_{v}}{MOD}</math>
- <math>\Delta\phi_{h}=\frac{4\pi k_{h}}{MOD}</math>
Hay dos casos particulares para cada una de las ondas horizontales o verticales:
- Curva circular. El ángulo de serpenteo es cero por lo que la amplitud también lo es. La forma que se obtiene es la de un sector circular, según el valor dado al parámetro offset. Además esta forma no cambia con el tiempo. Se calcula mediante la ecuación: <math>o=\frac{2\alpha}{MOD} </math>. En esta caso <math>\alpha</math> representa el ángulo del sector circular.
- Onda circular. El número de ondulaciones k es cero, por lo que la diferencia de fase <math>\Delta\Phi</math> también es cero y todas las articulaciones oscilan en fase. La forma varía con el tiempo entre un sector circular máximo y mínima, cuya amplitud viene dada por: <math>A=\frac{2\alpha}{MOD}</math>, donde <math>\alpha</math> es el ángulo del sector circular máximo.
Realización de los modos de caminar
Los modos de caminar del robot se establecen dando valores a sus parámetros del espacio de forma y transformándolos al espacio de control. En la siguiente tabla se muestran los diferentes movimientos posibles y las restricciones que deben cumplir sus parámetros
Modo de caminar | Restricciones | Parámetros libres | Descripción |
---|---|---|---|
Línea recta | <math>\alpha_{h}=0, o_{h}=0</math> | <math>\alpha_{v}, k_{v}</math> | No hay onda horizontal |
Giro | <math>\alpha_{h}=0, o_{h}\neq0</math> | <math>\alpha_{v}, k_{v}, o_{h}</math> | Curva circular en articulaciones horizontales. Ángulo del giro dado por el valor de <math>o_{h}</math> |
Desplazamiento lateral | <math>\alpha_{v}\ll\alpha_{h}, k_{v}=k_{h}, o_{h}=0, </math> <math>\Delta\Phi_{vh}=0</math> |
<math>\alpha_{h}, k_{h}</math> | Dos ondas serpentinoides con el mismo número de ondulaciones. La vertical con un ángulo de serpenteo mucho menor que la horizontal |
Desplazamiento lateral inclinado |
<math>\alpha_{v}=\alpha\sin\left(\beta\right)</math> |
<math>\alpha, k, \beta</math> | Dos ondas serpentinoides con mismo número ondulaciones. El parámetro beta determina la inclinación |
Rotación en S | <math>\alpha_{v}\ll\alpha_{h}, k_{v}=2k_{h}</math> <math>\Delta\Phi_{vh}=0, o_{h}=0</math> |
<math>\alpha_{h}, k_{h}</math> | Dos ondas serpentinoides. La vertical con ángulo de serpenteo mucho menor que el de la horizontal y con el doble de ondlulaciones |
Rotación en U | <math>\alpha_{v}\ll\alpha_{h}, k_{h}=0, k_{v}=2</math> <math>\Delta\Phi_{vh}=0, o_{h}=0</math> |
<math>\alpha_{h}</math> | Serpentinoide vertical con ángulo de serpenteo pequeño La onda horizontal es circular |
Remero | <math>\alpha_{v}=\alpha_{h}<\alpha_{min}, k_{v}=0, k_{h}=0, </math> <math>\Delta\Phi_{vh}=90, o_{h}=0 </math> |
--- | Dos ondas circulares iguales, con parámetros <math>\alpha</math> pequeños |
Rodar | <math>\alpha_{v}=\alpha_{h}>\alpha_{min}, k_{v}=0, k_{h}=0</math> <math>\Delta\Phi_{vh}=90, o_{h}=0 </math> |
<math>\alpha</math> | Dos ondas circulares iguales. El parámetro <math>\alpha</math> determina el ángulo del sector circular que adopta el robot al rodar |
En la siguiente tabla se muestran los valores que se han usado para el ejemplo presentado en este capítulo:
Línea recta | <math>\alpha_{v}=20</math>, <math>k_{v}=2</math>, <math>\alpha_{h}=0</math>, <math>o_{h}=0</math> |
Giro | <math>\alpha_{v}=20</math>, <math>k_{v}=2</math>, <math>\alpha_{h}=180</math> |
Desplazamiento lateral | <math>\alpha_{v}=5</math>, <math>k_{v}=1</math>, <math>\alpha_{h}=40</math>,<math>k_{h}=1</math>, <math>o_{h}=0</math>, <math>\Delta\Phi_{vh}=0</math> |
Desplazamiento lateral inclinado | <math>\alpha=40</math>, <math>\beta=45</math>, <math>k=1</math>, <math>o_{h}=0</math>, <math>\Delta\Phi_{vh}=0, o_{h}=0 </math> |
Rotación en S | <math>\alpha_{v}=5</math>, <math>k_{v}=2</math>, <math>\alpha_{h}=40</math>,<math>k_{h}=1</math>, <math>o_{h}=0</math>, <math>\Delta\Phi_{vh}=0 </math> |
Rotación en U | <math>\alpha_{v}=5</math>, <math>k_{v}=2</math>, <math>\alpha_{h}=180</math>,<math>k_{h}=0</math>, <math>o_{h}=0</math>, <math>\Delta\Phi_{vh}=0</math> |
Remero | <math>\alpha_{v}=12</math>, <math>k_{v}=0</math>, <math>\alpha_{h}=180</math>,<math>k_{h}=0</math>, <math>o_{h}=0</math>, <math>\Delta\Phi_{vh}=90</math> |
Rodar | <math>\alpha_{v}=180</math>, <math>k_{v}=0</math>, <math>\alpha_{h}=180</math>,<math>k_{h}=0</math>, <math>o_{h}=0</math>, <math>\Delta\Phi_{vh}=90</math> |
Explicación del código
Función Robot_new()
Es la función encargada de construir el modelo ODE de la serpiente. Construye robots del grupo cabeceo-viraje, de número de módulos MOD, con MOD>=1. Se divide en varias partes:
Primero se construye el cuerpo izquierdo, con su geometría asociada:
snake->body_left = dBodyCreate(world); dBodySetPosition(snake->body_left, 0.0, -L/4, H/2); dMassSetBoxTotal (&m, MASS/2, W,L/2,H); dBodySetMass (snake->body_left,&m); snake->geom_left = dCreateBox (space, W,L/2, H); dGeomSetBody (snake->geom_left,snake->body_left);
Luego los cuerpos centrales, que tienen dos geometrías asociadas, en desplazadas -L/4 y L/4 en el eje y respecto al centro de masas del cuerpo:
for (int i=0; i<MOD-1; i++) { snake->body[i]=dBodyCreate(world); dBodySetPosition(snake->body[i], 0.0, -L*(i+1), H/2); dMassSetBoxTotal (&m, MASS, W,L/2,H); dBodySetMass (snake->body[i],&m); snake->geom1[i] = dCreateBox (space, W,L/2, H); snake->geom2[i] = dCreateBox (space, W,L/2, H); dGeomSetBody (snake->geom1[i],snake->body[i]); dGeomSetBody (snake->geom2[i],snake->body[i]); dGeomSetOffsetPosition (snake->geom1[i], 0.0, L/4, 0.0); dGeomSetOffsetPosition (snake->geom2[i], 0.0, -L/4, 0.0); }
Ahora el cuerpo derecho:
snake->body_right = dBodyCreate(world); dBodySetPosition(snake->body_right, 0.0, -MOD*L+L/4, H/2); dMassSetBoxTotal (&m, MASS/2, W,L/2,H); dBodySetMass (snake->body_right,&m); snake->geom_right = dCreateBox (space, W,L/2, H); dGeomSetBody (snake->geom_right,snake->body_right);
Después se añaden las articulaciones centrales. Las pares rotan alrededor del eje x, las impares alrededor del z:
for (int i=1; i<=MOD-2; i++) { snake->joint[i]=dJointCreateHinge (world, 0); dJointAttach (snake->joint[i],snake->body[i-1],snake->body[i]); if (i%2==0) dJointSetHingeAxis (snake->joint[i], 1,0,0); //-- Pitch else dJointSetHingeAxis (snake->joint[i], 0,0,1); //-- Yaw dJointSetHingeAnchor (snake->joint[i], 0, -i*L-L/2, H/2); dJointSetHingeParam(snake->joint[i], dParamFMax, TORQUE); dJointSetHingeParam(snake->joint[i], dParamVel, 0.0); }
y por último las articulaciones izquierda y derecha. Se ha separado en casos diferentes para que se pueda construir robots de cual número de módulos (MOD>=1).
Para la articulación izquierda:
snake->joint[0]=dJointCreateHinge (world, 0); //-- If the robot consist of only one module, the left joint is //-- connected to the left and right bodies. //-- If not, it is connected to the left body and the next if (MOD==1) dJointAttach (snake->joint[0],snake->body_left,snake->body_right); else dJointAttach (snake->joint[0],snake->body_left,snake->body[0]); dJointSetHingeAxis (snake->joint[0], 1,0,0); dJointSetHingeAnchor (snake->joint[0], 0, -L/2, H/2); dJointSetHingeParam(snake->joint[0], dParamFMax, TORQUE); dJointSetHingeParam(snake->joint[0], dParamVel, 0.0);
Para la articulación derecha:
//-- Create the joint in the right. It only should be created if there //-- are more than one modules in the robot. if (MOD>1) { snake->joint[MOD-1]=dJointCreateHinge (world, 0); dJointAttach (snake->joint[MOD-1],snake->body[MOD-2],snake->body_right); if (MOD%2==0) dJointSetHingeAxis (snake->joint[MOD-1], 0,0,1); //-- Yaw else dJointSetHingeAxis (snake->joint[MOD-1], 1,0,0); //-- Pitch dJointSetHingeAnchor (snake->joint[MOD-1], 0, -MOD*L+L/2, H/2); dJointSetHingeParam(snake->joint[MOD-1], dParamFMax, TORQUE); dJointSetHingeParam(snake->joint[MOD-1], dParamVel, 0.0); }
Función Robot_Render()
Es la función que se encarga de dibujar la serpiente en pantalla. Hay que representar las 2*MOD geometrías que tiene el robot. Dependiendo de que la geometría pertenezca a un módulo par o impar, el color utilizado será amarillo o rojo.
Primero se dibuja la geomtría izquierda, asociada al cuerpo izquierdo, en color amarillo:
dsSetColor (1,1,0); drawGeom (snake->geom_left);
A continuación se dibujan todas las geomtrías de los cuerpos centrales, cambiando el color:
for (int i=0; i<MOD-1; i++) { if (i%2==0) G=1; else G=0; //-- Draw the first box dsSetColor (1,G,0); drawGeom(snake->geom1[i]); //-- Draw the second box dsSetColor (1,!G,0); drawGeom(snake->geom2[i]); }
Por último se dibuja la geometría derecha. Su color será diferente al de la geometría anterior:
dsSetColor (1,!G,0); drawGeom (snake->geom_right);
Función servos_sim()
Esta función es igual que la que se implementó en el capítulo anterior, pero ahora en vez de dos servos hay que simular MOD. Por ello, sólo cambia la definición del bucle:
for (int i=0; i<MOD; i++) {
El resto del código es igual
Función sequence_generation()
Esta función se encarga de generar las posiciones de referencia para todos los servos. Las ecuaciones de los osciladores son diferentes dependiendo de si son verticales (módulos impares) u horizontales (módulos pares):
for (int i=0; i<MOD; i++) { if (i%2==0) { phase = PDv*(i/2) + F0; snake.servo_ref_pos[i]=Av*sin(2*M_PI*n/N + DEG2RAD(phase) ); } else { phase = PDh*(i-1)/2 + PDvh + F0; snake.servo_ref_pos[i]=Ah*sin(2*M_PI*n/N + DEG2RAD(phase))+Oh; } } n = (n + 1) % N;
Funciones para implementar la locomoción
Para mover el robot hay que especificar el valor de los diferentes parámetros del espacio de formas y calcular a partir de ellos los parámetros de los osciladores del espacio de control. Dependiendo del modo de caminar que se quiere lograr, las funciones para ello son:
- Movimiento en línea recta:
void straight(dReal alfa,dReal k) { Ah=0; Oh=0; F0=0; Av=2*alfa*sin(2*M_PI*k/MOD); PDv=direction*(4*M_PI*k/MOD)*180/M_PI; }
- Giros:
void turning(dReal alfah, dReal kv) { Ah=0; F0=0; Oh=2*alfah/MOD; Av=2*20*sin(2*M_PI*kv/MOD); PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; }
- Desplazamiento lateral:
void side_winding(dReal alfah, dReal kh) { dReal kv=kh; Av=2*5*sin(2*M_PI*kv/MOD); PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; Oh=0; Ah=2*alfah*sin(2*M_PI*kh/MOD); PDh=PDv; PDvh=0; }
- Desplazamiento lateral inclinado:
void inclined_side_winding(dReal alfa, dReal k) { dReal alfav = alfa*sin(45); dReal alfah = alfav; dReal kv=k; dReal kh=k; Av=2*alfav*sin(2*M_PI*kv/MOD); PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; Oh=0; Ah=2*alfah*sin(2*M_PI*kh/MOD); PDh=PDv; PDvh=0; }
- Rotación en S:
void rotating_S(dReal alfah, dReal kh) { dReal kv=2*kh; Av=2*5*sin(2*M_PI*kv/MOD); PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; Oh=0; Ah=2*alfah*sin(2*M_PI*kh/MOD); PDh=direction*(4*M_PI*kh/MOD)*180/M_PI; PDvh=0; }
- Rotación en U:
void rotating_U(dReal alfa) { dReal kv=2; Av=2*5*sin(2*M_PI*kv/MOD); PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; Oh=0; Ah=2*alfa/MOD; PDh=0; PDvh=direction*90; }
- Remero:
void flapping() { dReal alfa=12; Av=2*alfa/MOD; Ah=Av; PDv=0; PDh=0; PDvh=direction*90; Oh=0; }
- Rodar:
void rotating_S(dReal alfah, dReal kh) { dReal kv=2*kh; Av=2*5*sin(2*M_PI*kv/MOD); PDv=direction*(4*M_PI*kv/MOD)*180/M_PI; Oh=0; Ah=2*alfah*sin(2*M_PI*kh/MOD); PDh=direction*(4*M_PI*kh/MOD)*180/M_PI; PDvh=0; }
Función Command()
Esta es la función que selecciona el modo de caminar según la tecla pulsada por el usuario. Cada vez que se pulsa una tecla, el sentido del movimiento del robot se cambia. Se invoca a las funciones definidas en el apartado anterior:
if (cmd=='1') { direction=direction*-1; straight(20,2); } else if (cmd=='2') { direction=direction*-1; turning(180,2); } else if (cmd=='3') { direction=direction*-1; side_winding(40,1); } else if (cmd=='4') { direction=direction*-1; inclined_side_winding(40,1); } else if (cmd=='5') { direction=direction*-1; flapping(); } else if (cmd=='6') { direction=direction*-1; rotating_S(40,1); } else if (cmd=='7') { direction=direction*-1; rotating_U(180); } else if (cmd=='8') { direction=direction*-1; rolling(180); } else if (cmd=='q') { dsStop(); }
Enlaces
|
|