#!/usr/bin/python # -*- coding: iso-8859-15 -*- #***********************************************************************# #* gravedad_colision.py. Rafael Treviño. Junio 2006 *# #*---------------------------------------------------------------------*# #* LICENCIA GPL *# #*---------------------------------------------------------------------*# #* Ejemplo "hola mundo" del motor fisico ODE (Open Dynamics Engine) *# #* Es similar al ejemplo gravedad, pero se ha anadido deteccion de *# #* colisiones. Se crea un mundo virtual, se crea un suelo infinitao, *# #* se situa una "caja" a una cierta altura y se comienza la simulacion *# #* La caja caera hasta que choque con el suelo. La salida del programa *# #* es la posicion de la caja en los diferentes instantes de tiempo *# #*---------------------------------------------------------------------*# #* Este programa es para consola, no hay representacion grafica en 3D *# #*---------------------------------------------------------------------*# #* Los datos devueltos se pueden visualizar con Octave#Matlab *# #* Ejemplo de uso: *# #* $ gravedad_colision.py > func.m *# #* $ octave func.m *# #***********************************************************************# import ode from sys import stdout #*********************************************# #* Algunas constantes usadas en el programa *# #*********************************************# ##-- Numero maximo de puntos de contacto. Es para la deteccion de ##-- colisiones. MAX_CONTACTS = 4 ##-- Numero de instantes que queremos simular. Este valor se puede ##-- cambiar TICKS = 200 ##-- Estructura que representa los objetos del universo ##-- Hay dos tipos de elementos: ##-- * Cuerpos (bodys): Tienen la informacion sobre posicion, ##-- orientacion, velocidad lineal y velocidad de rotacion ##-- * Elementos geometricos: Determinal la forma y se usan para ##-- las colisiones ##-- Un objeto esta formado por el cuerpo y su geometria (o la ##-- composicion de varias geometrias. ##-- Se usará una tupla de la forma MyObject = (body, geom) ##********************************************# ## VARIABLES GLOBALES DEL PROGRAMA *# ##********************************************# ##-- Identificador para el mundo world = None ##-- Identificador del espacio (para colisiones) ##-- Para detectar las colisiones hay que crear un espacio con los ##-- elementos que pueden colisionar. space = None ##-- Identificador del grupo de articulaciones de los puntos de contactos ##-- Cuando hay una colision, se crean puntos de contacto entre las ##-- superficies y actuan como articulaciones: los objetos rotaran con ##-- respecto a estos puntos de contacto. contactgroup = None; ##-- El objeto que situamos en el mundo: la caja obj = (None, None); #***********************************************************************# #* CODIGO *# #***********************************************************************# #*********************************************# #* Crear la "caja" *# #* Se define la "caja" usando la API de ODE *# #* y se asocia al "mundo" *# #*********************************************# def Crear_objeto(): global obj, world, space ##-- Identificador para la masa. m = ode.Mass() ##-- Crear el Cuerpo y asociarlo al mundo body = ode.Body(world) ##-- Establecer la posicion inicial. Se pasan las coordenadas x,y,z ##-- En este ejemplo el objeto esta en el origen, a una altura de ##-- 4 unidades body.setPosition((0, 0, 4)) ##-- Establecer la rotacion ##-- Cada objeto puede estar rotado. Se crea una matriz de ##-- rotacion pasandole como parametros el vector que hace de ##-- eje de giro y el angulo que se rota. En este ejemplo se pasa ##-- el eje z y se rota '0' grados con respecto a el. ##-- Probar este mismo ejemplo con los parametros (R, 0, 0, 1, 45) # dRFromAxisAndAngle (R, 0, 0, 1, 0) # obj [0].setRotation (R) ##-- Establecer la masa del cuerpo ##-- Hay que especificar la masa total y las dimensiones ##-- del cubo. Como masa se toma 0.5 y las dimensiones de la caja ##-- son 0.5 x 0.5 x 0.1. Se pueden poner las que se quiera m.setBox(0.5, 0.5, 0.5, 0.1) body.setMass(m) ##-- Crear la geometria: un cubo que se anadira al espacio ##-- para detectar las colisiones. Las dimensiones son ##-- 0.5 x 0.5 x 0.1 geom = ode.GeomBox(space, (0.5, 0.5, 0.1)) ##-- Asociar el cuerpo con la geometria geom.setBody(body); ##-- Creamos el objeto obj = (body, geom) #****************************************************************# #* Funcion de retrollamada invocada por dSpaceCollide cuando *# #* dos objetos del espacio estan a punto de colisionar *# #* El ODE permite que los usuarios avanzados puedan implementar *# #* su propia rutina de colision. Para los usuarios no expertos *# #* que simplemente quieren una colision estandar, esta es la *# #* rutina que SIEMPRE deberan usar. No es solo valida para este *# #* ejemplo, vale para cualquiera. Pero no esta incluida en la *# #* libreria del ODE para que se pueda adaptar a otras *# #* necesidades. #****************************************************************# def nearCallback(data, o1, o2): global world, contactgroup ##-- Obtener los cuerpos asociados b1 = o1.getBody() b2 = o2.getBody() ##-- Si ya estan conectados por una articulacion, terminar if b1 and b2 and ode.areConnected(b1, b2): return ##-- Obtener los puntos de contacto, que en realidad son ##-- articulaciones (devuelve una lista) ##-- Para mas informacion consultar la documentacion de ODE ##-- Aqui se puede especificar el tipo de superficies, los ##-- coeficientes de rozamiento (mu), etc... contacts = ode.collide(o1, o2) ##-- Para cada punto de contacto crear una articulacion for c in contacts: ##-- Crear articulacion y meterla en el grupo contactgroup c.setMode(ode.ContactBounce|ode.ContactSoftCFM) c.setMu(ode.Infinity) c.setMu2(0) c.setBounce(0.1) c.setBounceVel(0.1) c.setSoftCFM(0.01) j = ode.ContactJoint(world, contactgroup, c) ##-- Establecer la articulacion entre los dos cuerpos j.attach(b1, b2); #***********************************************************************# #* Realizar un paso de simulacion. Primero se comprueba colisiones. Si *# #* hay alguna, se crean automaticamente (por medio de la funcion *# #* nearCallback, puntos de contactos que en realidad son articulaciones*# #* Despues se realiza un paso de la simulacion. Los objetos rotaran en *# #* contacto rotaran sobre los nuevos puntos de contacto creados. *# #* Finalmente se eliminan. *# #***********************************************************************# def simLoop(): global world, space, contactgroup ##-- Deteccion de colisiones. Determinar que pares de elementos ##-- geometricos estan a punto de colisionar. Se llama a la ##-- funcion de retrollamadanear Callback, pasando como argumento ##-- los dos elementos. space.collide(None, nearCallback) ##-- Realizar un paso de simulacion world.step(0.01) ## Eliminar todos los puntos de contacto (articulaciones) creados. contactgroup.empty() #***********************************************************************# #* MAIN *# #* Crear el mundo. Es un contenedor de todos los objetos a simular *# #* El mundo no sabe nada de como se dibujan los objetos *# #* En este ejemplo el mundo solo tiene un suelo y una "caja" *# #***********************************************************************# if __name__ == '__main__': ##-- Variable de posicion pos = None ##-- Crear mundo world = ode.World() ##-- Establecer la gravedad (gravedad terrestres: -9.81) world.setGravity((0, 0, -9.81)) ##-- Establecer parametro CFM ##-- Normalmente se deja siempre a este valor world.setCFM(1e-5) ##-- Establecer el modo auto-disabled por defecto ##-- Cualquier objeto que se encuentre en reposo se deshabilitara ##-- y no consumira recursos en la simulacion. Normalmente ##-- siempre se activara world.setAutoDisableFlag(1) ##-- Otros parametros... (consular documentacion) ##-- En principio siempre tendran esos valores world.setContactMaxCorrectingVel(0.1) world.setContactSurfaceLayer(0.001) ##-- Crear un espacio. Los espacios contienen los elementos ##-- geometricos sobre las que se quiere comprobar si hay colision ##-- o no. Se utilizan espacios para que la simulacion sea mas ##-- rapida space = ode.HashSpace() ##-- Crear un grupo de articulaciones ##-- Se utiliza para almacenar los puntos de contacto en una ##-- colision contactgroup = ode.JointGroup() ##-- Crear un plano infinito y meterlo en el espacio ##-- El plano se determina por su ecuacion del tipo: ##-- a*x + b*y + c*z = d, donde (a,b,c) es un vector unitario ##-- normal a su superficie ##-- En este ejemplo se crea el plano con vector (0,0,1), es ##-- decir, el plano: z=0 ##-- Este plano sera para nosotros el "suelo" floor = ode.GeomPlane(space, (0, 0, 1), 0) ##-- Crear la "Caja" y ponerla en el "mundo" Crear_objeto() ##-- Salida para Octave: ##-- La matriz z es la que se ira rellenando con los valores de ##-- la altura de la caja stdout.write('z = [') #*************************************# #** COMENZAR LA SIMULACION!!!! *# #*************************************# ##-- Este es el bucle principal. ##-- Se haran tantos pasos de simulacion como se indican en la ##-- constante TICKS for ticks in range(TICKS, 0, -1): ##-- Realizar un paso de la simulacion, ##-- comprobando colisiones simLoop(); ##-- Leer la altura del objeto e imprimirla ##-- Pos es el vector de posicion, que tiene 3 componentes: ##-- pos[0] --> x; pos[1]--> y; pos[2] --> z ##-- Solo nos interesa la altura a la que esta la caja ##-- (pos [2]) pos = obj[0].getPosition() stdout.write('%f, '%pos[2]) ##-- Simulacion finalizada: ##-- Imprimir la ultima posicion e ##-- Imprimir comandos octave para sacar grafica pos = obj[0].getPosition() print '%f];'%pos[2] print 't = 0:1:%d;'%TICKS print 'plot (t, z);' print 'grid on;' print 'pause;' #*************************************# #* FIN DE LA SIMULACION *# #*************************************# ##-- Destruir el grupo de articulaciones ##-- Destruir el espacio de colisiones ##-- Destruir el mundo con todos sus objetos (apocalipsis?) ##-- Implícitas por Python