[Pyode-commits] pyode/examples vehicle.py,NONE,1.1

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/pyode/pyode/examples
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv16076/examples

Added Files:
	vehicle.py 
Log Message:
- Added a new example to demonstrate the simulation of a vehicle.
- Added the wrapper ode.areConnected() for dAreConnected().
- Added support for the <mass><box/></mass> in the XODE parser.

--- NEW FILE: vehicle.py ---
# PyODE Example: VehicleDemo

import ode, xode.parser
import pygame
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *

info = """VehicleDemo

Controls:
  Left mouse button click: Apply upward force to the vehicle's chassis.
  W Key: Move forward.
  S Key: Move backward.
  A Key: Turn left.
  D Key: Turn right.
  Escape Key: Exit.
"""

# This XODE document contains the description of the vehicle, the ground plane
# and the ramp. The stack of boxes are created at runtime.

doc = '''<?xml version="1.0" encoding="iso-8859-1"?>
<xode>
  <world name="world">
    <space name="space">

      <geom name="ground">
        <plane a="0" b="1" c="0" d="-4" />
      </geom>

      <geom name="ramp">
        <transform>
          <position x="0" y="-2" z="-20" />
          <rotation>
            <euler x="-30" y="0" z="0" aformat="degrees" />
          </rotation>
        </transform>

        <box sizex="10" sizey="1" sizez="10" />
      </geom>

      <jointgroup name="vehicle_joints">

        <body name="chassis">
          <transform>
            <position x="0" y="0" z="0" />
          </transform>

          <geom name="chassis_geom">
            <box sizex="4" sizey="0.5" sizez="5" />
          </geom>

          <body name="wheel1_body">
            <transform>
              <position x="0" y="0" z="-2" />
            </transform>

            <geom name="wheel1_geom">
              <sphere radius="1" />
            </geom>

            <joint name="wheel1">
              <link1 body="chassis" />
              <hinge2>
                <anchor x="0" y="0" z="-2" />
                <axis x="0" y="1" z="0" FMax="50" LoStop="-0.78" HiStop="0.78"/>
                <axis x="1" y="0" z="0" FMax="5000"
                                        SuspensionERP="0.8"
                                        SuspensionCFM="0.5" />
              </hinge2>
            </joint>

            <mass>
              <mass_shape density="1">
                <sphere radius="1" />
              </mass_shape>
            </mass>
          </body>

          <body name="wheel2_body">
            <transform>
              <position x="2" y="0" z="2" />
            </transform>

            <geom name="wheel2_geom">
               <sphere radius="1" />
            </geom>

            <joint name="wheel2">
              <link1 body="chassis" />
              <hinge2>
                <anchor x="2" y="0" z="2" />
                <axis x="0" y="1" z="0" FMax="0" LoStop="0" HiStop="0" />
                <axis x="1" y="0" z="0" FMax="5000"
                                        SuspensionERP="0.8"
                                        SuspensionCFM="0.5" />
              </hinge2>
            </joint>

            <mass>
              <mass_shape density="1">
                <sphere radius="1" />
              </mass_shape>
            </mass>
          </body>

          <body name="wheel3_body">
            <transform>
              <position x="-2" y="0" z="2" />
            </transform>

            <geom name="wheel3_geom">
              <sphere radius="1" />
            </geom>

            <joint name="wheel3">
              <link1 body="chassis" />
              <hinge2>
                <anchor x="-2" y="0" z="2" />
                <axis x="0" y="1" z="0" FMax="0" LoStop="0" HiStop="0" />
                <axis x="1" y="0" z="0" FMax="5000"
                                        SuspensionERP="0.8"
                                        SuspensionCFM="0.5" />
              </hinge2>
            </joint>

            <mass>
              <mass_shape density="1">
                <sphere radius="1" />
              </mass_shape>
            </mass>
          </body>

          <mass>
            <mass_shape density="30">
              <box sizex="6" sizey="5" sizez="3" />
            </mass_shape>
          </mass>

        </body>
      </jointgroup>

    </space>
  </world>
</xode>
'''

class VehicleDemo:
    """
    Vehicle Demo
    """

    fps = 50.0
    cameraDistance = 10.0
    vel = 50.0
    turn = 1.0
    clip = 100.0
    res = (640, 480)

    def __init__(self):
        """
        Initialises this.
        """

        self._initOpenGL()
        self._loadObjects()
        self._buildWall()

        self._cjoints = ode.JointGroup()

        self._xRot = 0.0
        self._yRot = 0.0

        self._xCoeff = 360.0 / 480.0
        self._yCoeff = 360.0 / 640.0

        self._vel = 0.0
        self._turn = 0.0

    def _loadObjects(self):
        p = xode.parser.Parser()
        root = p.parseString(doc)

        self.world = root.namedChild('world').getODEObject()
        self.space = root.namedChild('space').getODEObject()
        self.ground = root.namedChild('ground').getODEObject()
        self.chassis = root.namedChild('chassis').getODEObject()
        self.wheel1 = root.namedChild('wheel1').getODEObject()
        self.wheel2 = root.namedChild('wheel2').getODEObject()
        self.wheel3 = root.namedChild('wheel3').getODEObject()

        self.root = root
        self.world.setGravity((0, -9.81, 0))

        # transverse the xode parse tree to make a list of all geoms and joints
        self._geoms = []
        self._joints = []

        def transverse(node):
            obj = node.getODEObject()
            if (isinstance(obj, ode.GeomObject)):
                self._geoms.append(obj)
            if (isinstance(obj, ode.Joint)):
                self._joints.append(obj)

            for child in node.getChildren():
                transverse(child)

        transverse(root)

    def _buildWall(self):
        """
        Create the wall of stacked boxes.
        """

        def makeBox(x, y):
            body = ode.Body(self.world)
            body.setPosition((x, y, -40))

            geom = ode.GeomBox(self.space, lengths=(0.5, 0.5, 0.5))
            geom.setBody(body)

            self._geoms.append(geom)

        for x in range(-4, 5):
            for y in range(7):
                makeBox(x, y)

    def _initOpenGL(self):
        """
        Initialise the scene.
        """

        # Create a window
        pygame.init()
        screen = pygame.display.set_mode(self.res,
                                         pygame.OPENGL | pygame.DOUBLEBUF)
        pygame.display.set_caption('PyODE Vehicle Demo')
        pygame.mouse.set_visible(False)

        glViewport(0, 0, self.res[0], self.res[1])
        glClearColor(0.8, 0.8, 0.9, 0)

        glEnable(GL_DEPTH_TEST)
        glEnable(GL_LIGHTING)
        glEnable(GL_NORMALIZE)
        glShadeModel(GL_FLAT)

    def _extractMatrix(self, geom):
        """
        Return a 4x4 matrix (represented by a 16-element tuple) created by
        combining the geom's rotation matrix and position.
        """

        x, y, z = geom.getPosition()
        rot = geom.getRotation()
        return (rot[0], rot[3], rot[6], 0.0,
                rot[1], rot[4], rot[7], 0.0,
                rot[2], rot[5], rot[8], 0.0,
                x, y, z, 1.0)

    def _renderGeom(self, geom):
        """
        Render either a ode.GeomBox or ode.GeomSphere object.
        """

        allowed = [ode.GeomBox, ode.GeomSphere]
        ok = False
        for klass in allowed:
            ok = ok or isinstance(geom, klass)
        if (not ok):
            return

        glPushMatrix()
        glMultMatrixd(self._extractMatrix(geom))

        glMaterialfv(GL_FRONT, GL_SPECULAR, (0.0, 0.0, 0.0))

        if (isinstance(geom, ode.GeomBox)):
            sx, sy, sz = geom.getLengths()
            glScale(sx, sy, sz)
            glutSolidCube(1)
        elif (isinstance(geom, ode.GeomSphere)):
            r = geom.getRadius()
            glutSolidSphere(r, 20, 20)

        glPopMatrix()

    def _renderGround(self):
        """
        Renders the ground plane.
        """

        # Draw a quad at the position of the vehicle that extends to the
        # clipping planes.

        normal, d = self.ground.getParams()
        x, y, z = self.chassis.getPosition()

        glPushMatrix()
        glTranslate(x, 0.0, z)

        glMaterialfv(GL_FRONT, GL_SPECULAR, (0.0, 1.0, 0.0))

        glBegin(GL_QUADS)
        glColor3(0.0, 1.0, 0.0)
        glNormal3f(*normal)
        glVertex3f(-self.clip, d, -self.clip)
        glNormal3f(*normal)
        glVertex3f(self.clip, d, -self.clip)
        glNormal3f(*normal)
        glVertex3f(self.clip, d, self.clip)
        glNormal3f(*normal)
        glVertex3f(-self.clip, d, self.clip)
        glEnd()

        glPopMatrix()

    def _setCamera(self):
        """
        Position the camera to C{self.cameraDistance} units behind the
        vehicle's current position and rotated depending on the mouse position.
        """

        aspect = float(self.res[0]) / float(self.res[1])

        x, y = pygame.mouse.get_pos()
        self._xRot = (y - self.res[1]/2) * self._xCoeff
        self._yRot = (x - self.res[0]/2) * self._yCoeff
        if (self._xRot < 0):
            self._xRot = 0

        glMatrixMode(GL_PROJECTION)
        glLoadIdentity()
        glFrustum(-aspect, aspect, -1.0, 1.0, 1.5, self.clip)

        glLightfv(GL_LIGHT0, GL_POSITION, (-5.0, 10.0, 0, 0))
        glLightfv(GL_LIGHT0,GL_DIFFUSE, (1.0, 1.0, 1.0, 1.0))
        glLightfv(GL_LIGHT0,GL_SPECULAR, (1.0, 1.0, 1.0, 1.0))
        glEnable(GL_LIGHT0)

        glMatrixMode(GL_MODELVIEW)
        glLoadIdentity()

        # Set the camera angle to view the vehicle
        glTranslate(0.0, 0.0, -self.cameraDistance)
        glRotate(self._xRot, 1, 0, 0)
        glRotate(self._yRot, 0, 1, 0)

        # Set the camera so that the vehicle is drawn in the correct place.
        x, y, z = self.chassis.getPosition()
        glTranslate(-x, -y, -z)

    def render(self):
        """
        Render the current simulation state.
        """

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
        self._renderGround()

        self._setCamera()
        for geom in self._geoms:
            self._renderGeom(geom)

        glFlush()
        pygame.display.flip()

    def _keyDown(self, key):
        if (key == pygame.K_w):
            self._vel = self.vel
        elif (key == pygame.K_a):
            self._turn = -self.turn
        elif (key == pygame.K_d):
            self._turn = self.turn
        elif (key == pygame.K_s):
            self._vel = -self.vel
        elif (key == pygame.K_ESCAPE):
            self._running = False

    def _keyUp(self, key):
        if (key == pygame.K_w):
            self._vel = 0.0
        elif (key == pygame.K_a):
            self._turn = 0.0
        elif (key == pygame.K_d):
            self._turn = 0.0
        elif (key == pygame.K_s):
            self._vel = 0.0

    def doEvents(self):
        """
        Process any input events.
        """

        events = pygame.event.get()

        for e in events:
            if (e.type == pygame.QUIT):
                self._running = False
            elif (e.type == pygame.KEYDOWN):
                self._keyDown(e.key)
            elif (e.type == pygame.KEYUP):
                self._keyUp(e.key)
            elif (e.type == pygame.MOUSEBUTTONDOWN):
                if (e.button == 1):
                    self.chassis.addForce((0.0, 500000, 0.0))

    def _nearcb(self, args, geom1, geom2):
        """
        Create contact joints between colliding geoms.
        """

        body1, body2 = geom1.getBody(), geom2.getBody()
        if (body1 is None):
            body1 = ode.environment
        if (body2 is None):
            body2 = ode.environment

        if (ode.areConnected(body1, body2)):
            return

        contacts = ode.collide(geom1, geom2)

        for c in contacts:
            c.setBounce(0.2)
            c.setMu(10000)
            j = ode.ContactJoint(self.world, self._cjoints, c)
            j.attach(body1, body2)

    def run(self):
        """
        Start the demo. This method will block until the demo exits.
        """

        clock = pygame.time.Clock()
        self._running = True

        while self._running:
            self.doEvents()

            # Steering
            self.wheel1.setParam(ode.ParamVel, self._turn)

            # Engine
            self.wheel1.setParam(ode.ParamVel2, self._vel)
            self.wheel2.setParam(ode.ParamVel2, self._vel)
            self.wheel3.setParam(ode.ParamVel2, self._vel)

            self.space.collide((), self._nearcb)
            self.world.step(1/self.fps)
            self._cjoints.empty()
            self.render()

            # Limit the FPS.
            clock.tick(self.fps)

if (__name__ == '__main__'):
    print info
    demo = VehicleDemo()
    demo.run()