def setup(world, body, mass, space, g_world, body_geom, i, c1, boat_model): world.setGravity((0, 0, -9.81)) # this is the mass of the boat "boat_mass", this also sets the inertia based on a box with length_x, length_y, length_z mass.setBoxTotal(boat_mass, boat_length, boat_width, boat_height) body.setMass(mass) body.setPosition(initial_position) # the last unit is the initial rotation of the body position in radians body.setQuaternion(smh.axisangle_to_quat(v(0, 0, 1), math.pi)) #body.setQuaternion(smh.axisangle_to_quat(v(0, 0, 1), 0)) body_geom.setBody(body) lake_mesh = srh.mesh_from_obj(roslib.packages.resource_file('navigator_sim_model', 'models', 'lake.obj')) lake_geom = ode.GeomTriMesh(lake_mesh.ode_trimeshdata, space) # TODO: Add obstacles here g_world.objs.append(lake_mesh) g_world.objs.append(boat_model) g_world.objs.append(srh.VectorField(get_water_vel)) geoms = [] buoy_list = [v(54.5, -33.9, 0), v(53.5, -37.4, 0), v(38.0, -29.6, 0), v(35.7, -33.5, 0)] for pos in buoy_list: newbuoy_mesh = srh.mesh_from_obj(roslib.packages.resource_file('navigator_sim_model', 'models', 'green_buoy.obj')) newbuoy_mesh = newbuoy_mesh.translate(pos) g_world.objs.append(newbuoy_mesh) geoms.append(ode.GeomTriMesh(newbuoy_mesh.ode_trimeshdata, space)) i.init(g_world)
def loadModel(self, filename): mesh = loadObj(filename) body = ode.Body(self.world) self.model = ode.GeomTriMesh(mesh, self.space[0]) self.model.setBody(body) # rotate so that z becomes top self.addGeom(self.model, "model") self.model.setQuaternion( (0.7071067811865476, 0.7071067811865475, 0, 0)) body = ode.Body(self.world) ray = ode.GeomRay(self.space[0], 10000) ray.setBody(body) ray.set((0, 0, 1), (0, 0, 100000)) self.addGeom(ray, "XXXXXXX") self.GetProperty(ray).SetColor(1, 0, 0) self.GetProperty(ray).SetLineWidth(30) print(self.GetProperty(ray)) self.update()
def get_parallelepiped_geom(low_points, high_points, space=None): triangles = ( # front side Triangle(low_points[0], low_points[1], high_points[0]), Triangle(low_points[1], high_points[1], high_points[0]), # right side Triangle(low_points[1], low_points[2], high_points[1]), Triangle(low_points[2], high_points[2], high_points[1]), # back side Triangle(low_points[2], low_points[3], high_points[2]), Triangle(low_points[3], high_points[3], high_points[2]), # left side Triangle(low_points[3], low_points[0], high_points[3]), Triangle(low_points[0], high_points[0], high_points[3]), # bottom side Triangle(low_points[1], low_points[0], low_points[2]), Triangle(low_points[3], low_points[2], low_points[0]), # high side Triangle(high_points[0], high_points[1], high_points[2]), Triangle(high_points[2], high_points[3], high_points[0]), ) mesh = ode.TriMeshData() vertices, faces = convert_triangles_to_vertices_faces(triangles) mesh.build(vertices, faces) geom = ode.GeomTriMesh(mesh, space) return geom
def setTerrainData(self, data_): self._terrainData = data_ verts, faces = generateTerrainVerts(self._terrainData, translateX=self._terrainStartX) del self._terrainMeshData self._terrainMeshData = ode.TriMeshData() self._terrainMeshData.build(verts, faces) del self._floor self._floor = ode.GeomTriMesh(self._terrainMeshData, self._space)
def add_mesh(self, triangles, position=None): if position is None: position = (0, 0, 0) mesh = ode.TriMeshData() vertices, faces = convert_triangles_to_vertices_faces(triangles) mesh.build(vertices, faces) geom = ode.GeomTriMesh(mesh, self._space) self._add_geom(geom, position) self._dirty = True
def __init__(self, world, space, position, density, mesh_info): '''Mesh object - Dynamics are *unsupported* right now This spawns a static object that can only be collided with and does not move TODO: Fix that ''' # Create a mesh geometry for collision detection mesh_vertices, mesh_faces, mesh_normals, texcoords = mesh_info meshdata = ode.TriMeshData() meshdata.build(mesh_vertices, mesh_faces) self.geom = ode.GeomTriMesh(meshdata, space)
def create_body(density, r, h): """ Create a cylindrical body and its corresponding geom. """ # Create body body = ode.Body(world) M = ode.Mass() M.setCylinder(density, 2, r, h) body.setMass(M) # Create a trimesh geom for collision detection tm = ode.TriMeshData() tm.build(mesh.verts, mesh.faces) geom = ode.GeomTriMesh(tm, space) geom.setBody(body) return (body, geom)
def DefineMeshTotal(self, TotalMass, CenterOfMass, InertiaMatrix, Mesh): """Define this element as a mesh. The Mesh must be a vpyode.Mesh class object. The cg and IntertiaMaxtrix are defined in world coordinates.""" if self._hasGeom: self._geom = ode.GeomTriMesh(Mesh, None) DisplayElement.SetDisplayObject(self, Mesh.MakeMeshObject()) # Setup the mass properties self._mass = ode.Mass() cgx, cgy, cgz = CenterOfMass (I11, I12, I13), (I21, I22, I23), (I31, I32, I33) = InertiaMatrix self._mass.setParameters(TotalMass, cgx, cgy, cgz, I11, I22, I33, I12, I13, I23) return self
def _compute_trimesh(self, quality_factor=1.0): ''' Connect a ode.Trimesh to this body. The mesh is generated from the MSH subpackage. vertices lits and faces indices are passed to the trimesh. The default mesh precision is divided by the quality factor: - if quality_factor>1, the mesh will be more precise, i.e. more triangles (more precision, but also more memory consumption and time for the mesher, - if quality_factor<1, the mesh will be less precise. By default, this argument is set to 1 : the default precision of the mesher is used. ''' a_mesh = QuickTriangleMesh() a_mesh.set_shape(self._shape) # define precision for the mesh a_mesh.set_precision(a_mesh.get_precision() / quality_factor) # then compute the mesh a_mesh.compute() # The results of the mesh computation are passed to the trimesh manager #a_mesh.build_lists() vertices = a_mesh.get_vertices() # Before we set the vertices list to the trimesh, we must translate them: # TODO : there might be a simplier way to achieve that point for i in range(len(vertices)): v = vertices[i] v_x = v[0] - self.x_g v_y = v[1] - self.y_g v_z = v[2] - self.z_g vertices[i] = [v_x, v_y, v_z] # faces also have to be reshaped faces = a_mesh.get_faces() i = 0 f = [] while i < len(faces): f.append([i, i + 1, i + 2]) i = i + 3 # Create the trimesh data td = ode.TriMeshData() td.build(vertices, f) self._collision_geometry = ode.GeomTriMesh( td, self._parent_context.get_collision_space()) self._collision_geometry.setBody(self)
def _setup_cushion_meshes(self): (headCushionGeom, leftHeadCushionGeom, rightHeadCushionGeom, footCushionGeom, leftFootCushionGeom, rightFootCushionGeom) = self.cushion_geoms tri_mesh_data = ode.TriMeshData() tri_mesh_data.build( self.table.headCushionGeom.attributes['vertices'].reshape( -1, 3).tolist(), self.table.headCushionGeom.indices.reshape(-1, 3)) self.head_cushion_geom = ode.GeomTriMesh(tri_mesh_data, space=self.space) tri_mesh_data = ode.TriMeshData() tri_mesh_data.build( self.table.leftHeadCushionGeom.attributes['vertices'].reshape( -1, 3).tolist(), self.table.leftHeadCushionGeom.indices.reshape(-1, 3)) self.left_head_cushion_geom = ode.GeomTriMesh(tri_mesh_data, space=self.space) tri_mesh_data = ode.TriMeshData() tri_mesh_data.build( self.table.rightHeadCushionGeom.attributes['vertices'].reshape( -1, 3).tolist(), self.table.rightHeadCushionGeom.indices.reshape(-1, 3)) self.right_head_cushion_geom = ode.GeomTriMesh(tri_mesh_data, space=self.space) tri_mesh_data = ode.TriMeshData() tri_mesh_data.build( self.table.footCushionGeom.attributes['vertices'].reshape( -1, 3).tolist(), self.table.footCushionGeom.indices.reshape(-1, 3)[:, ::-1]) self.foot_cushion_geom = ode.GeomTriMesh(tri_mesh_data, space=self.space) tri_mesh_data = ode.TriMeshData() tri_mesh_data.build( self.table.leftFootCushionGeom.attributes['vertices'].reshape( -1, 3).tolist(), self.table.leftFootCushionGeom.indices.reshape(-1, 3)[:, ::-1]) self.left_foot_cushion_geom = ode.GeomTriMesh(tri_mesh_data, space=self.space) tri_mesh_data = ode.TriMeshData() tri_mesh_data.build( self.table.rightFootCushionGeom.attributes['vertices'].reshape( -1, 3).tolist(), self.table.rightFootCushionGeom.indices.reshape(-1, 3)[:, ::-1]) self.right_foot_cushion_geom = ode.GeomTriMesh(tri_mesh_data, space=self.space)
def setNode(self, node): super().setNode(node) aabb = node.getBoundingBox() if not aabb.isValid(): return if not self._body: self._body = ode.Body(self._world) self._body.setMaxAngularSpeed(0) mass = ode.Mass() mass.setBox(5.0, Helpers.toODE(aabb.width), Helpers.toODE(aabb.height), Helpers.toODE(aabb.depth)) self._body.setMass(mass) if not self._geom: if node.getMeshData(): scale_matrix = Matrix() scale_matrix.setByScaleFactor(1.01) mesh = node.getMeshData().getTransformed(scale_matrix) self._trimesh = ode.TriMeshData() debug_builder = MeshBuilder() vertices = mesh.getVertices() indices = mesh.getConvexHull().simplices _fixWindingOrder(vertices, indices, debug_builder) self._trimesh.build(vertices / Helpers.ScaleFactor, indices) self._geom = ode.GeomTriMesh(self._trimesh, self._space) mb = MeshBuilder() for i in range(self._geom.getTriangleCount()): tri = self._geom.getTriangle(i) v0 = Helpers.fromODE(tri[0]) v1 = Helpers.fromODE(tri[1]) v2 = Helpers.fromODE(tri[2]) mb.addFace(v0=v0, v1=v1, v2=v2, color=Color(1.0, 0.0, 0.0, 0.5)) chn = SceneNode(node) chn.setMeshData(mb.build()) def _renderConvexHull(renderer): renderer.queueNode(chn, transparent=True) return True chn.render = _renderConvexHull n = SceneNode(node) n.setMeshData(debug_builder.build()) def _renderNormals(renderer): renderer.queueNode(n, mode=1, overlay=True) return True n.render = _renderNormals else: self._geom = ode.GeomBox(self._space, lengths=(Helpers.toODE(aabb.width), Helpers.toODE(aabb.height), Helpers.toODE(aabb.depth))) self._geom.setBody(self._body) self._body.setPosition(Helpers.toODE(node.getWorldPosition())) node.transformationChanged.connect(self._onTransformationChanged)
def __init__(self, world, space, shape, pos, size=[], bounciness=1, friction=0, vertices=None, indices=None): self.node3D = viz.addGroup() # If it is NOT linked it updates seld.node3D pose with pos/quat pose on each frame # If it is NOT linked it updates pos/quat pose with node3D pose on each frame # This allows a linked phsynode to be moved around by an external source via the node3D self.isLinked = 0 self.geom = 0 self.body = 0 self.parentWorld = [] self.parentSpace = [] self.bounciness = bounciness self.friction = friction # A list of bodies that it will stick to upon collision self.stickTo_gIdx = [] self.collisionPosLocal_XYZ = [] if shape == 'plane': # print 'phsEnv.createGeom(): type=plane expects pos=ABCD,and NO size. SIze is auto infinite.' self.geom = ode.GeomPlane(space, [pos[0], pos[1], pos[2]], pos[3]) self.parentSpace = space # No more work needed elif shape == 'sphere': #print 'Making sphere physNode' # print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS' ################################################################################################ ################################################################################################ # Define the Body: something that moves as if under the # influence of environmental physical forces self.geomMass = ode.Mass() # set sphere properties automatically assuming a mass of 1 and self.radius mass = 1.0 self.geomMass.setSphereTotal(mass, size) self.body = ode.Body(world) self.parentWorld = world self.body.setMass(self.geomMass) # geomMass or 1 ? self.body.setPosition(pos) # Define the Geom: a geometric shape used to calculate collisions #size = radius! self.geom = ode.GeomSphere(space, size) self.geom.setBody(self.body) self.parentSpace = space ################################################################################################ ################################################################################################ #elif shape == 'cylinder': elif ('cylinder' in shape): #print 'Making cylinder physNode' # print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS' ################################################################################################ ################################################################################################ # Define the Body: something that moves as if under the # influence of environmental physical forces radius = size[1] length = size[0] self.geomMass = ode.Mass() # set sphere properties automatically assuming a mass of 1 and self.radius mass = 1.0 if (shape[-2:] == '_X'): direction = 1 elif (shape[-2:] == '_Y'): direction = 2 else: direction = 3 # direction - The direction of the cylinder (1=x axis, 2=y axis, 3=z axis) self.geomMass.setCylinderTotal(mass, direction, radius, length) self.body = ode.Body(world) self.parentWorld = world self.body.setMass(self.geomMass) # geomMass or 1 ? self.body.setPosition(pos) # Define the Geom: a geometric shape used to calculate collisions #size = radius! self.geom = ode.GeomCylinder(space, radius, length) self.geom.setPosition(pos) self.geom.setBody(self.body) # This bit compensates for a problem with ODE # Note how the body is created in line with any axis # When I wrote this note, it was in-line with Y (direction=2) # The geom, however, can only be made in-line with the Z axis # This creates an offset to bring the two in-line vizOffsetTrans = viz.Transform() if (shape[-2:] == '_X'): vizOffsetTrans.setAxisAngle(1, 0, 0, 90) elif (shape[-2:] == '_Y'): vizOffsetTrans.setAxisAngle(0, 0, 1, 90) vizOffsetQuat = vizOffsetTrans.getQuat() odeRotMat = self.vizQuatToRotationMat(vizOffsetQuat) #print self.geom.getRotation() self.geom.setOffsetRotation(odeRotMat) self.parentSpace = space elif shape == 'box': ################################################################################################ ################################################################################################ # Define the Body: something that moves as if under the # influence of environmental physical forces length = size[1] width = size[2] height = size[0] self.geomMass = ode.Mass() # set sphere properties automatically assuming a mass of 1 and self.radius mass = 1.0 self.geomMass.setBoxTotal(mass, length, width, height) self.body = ode.Body(world) self.parentWorld = world self.body.setMass(self.geomMass) # geomMass or 1 ? self.body.setPosition(pos) # Define the Geom: a geometric shape used to calculate collisions #size = radius! self.geom = ode.GeomBox(space, [length, width, height]) self.geom.setPosition(pos) self.geom.setBody(self.body) self.parentSpace = space elif shape == 'trimesh': if (vertices == None or indices == None): print 'physNode.init(): For trimesh, must pass in vertices and indices' self.body = ode.Body(world) self.parentWorld = world self.body.setMass(self.geomMass) # geomMass or 1 ? self.body.setPosition(pos) triMeshData = ode.TrisMeshData() triMeshData.build(vertices, indices) self.geom = ode.GeomTriMesh(td, space) self.geom.setBody(self.body) ## Set parameters for drawing the trimesh body.shape = "trimesh" body.geom = self.geom self.parentSpace = space else: print 'physEnv.physNode.init(): ' + str( type) + ' not implemented yet!' return pass #print '**************UPDATING THE NODE *****************' self.updateNodeAct = vizact.onupdate(viz.PRIORITY_PHYSICS, self.updateNode3D)
def main(): InitGraphics() fps = 0 fpsdisplay = 0 starttime = pygame.time.get_ticks() fpsstring = 'none' clock = pygame.time.Clock() background = texture.Texture('data/background.png') camptexture = texture.Texture('data/basecamp.png') fuelstationtexture = texture.Texture('data/fuelstation.png') font = text.BMFont("data/font.fnt") foodtexture = texture.Texture('data/foodcrate.png') supplytexture = texture.Texture('data/supplycrate.png') fueltexture = texture.Texture('data/fuelcrate.png') rightgeartexture = texture.Texture('data/right_gear.png') leftgeartexture = texture.Texture('data/left_gear.png') shiptexture = texture.Texture('data/ship.png') world, space, contactgroup = physics.InitPhysics() basecampHunger = 0 basecampEnergy = 100 basecampSupply = 100 basecampPopulation = 4000 ship = physics.Enterprise(world, space, (200, 200, 0), (rightgeartexture, leftgeartexture, shiptexture)) fuelstation = physics.FuelStation(world, space, (1200, 50, 0), fuelstationtexture) basecamp = physics.BaseCamp(world, space, (2200, 50, 0), camptexture) supplycrates = [] # add 10 of each crate to the world for index in range(0, 10): xrand = random.randrange(0, 200) yrand = random.randrange(0, 25) yrand += 400 xrand += index * 10 yrand += index * 10 fuelcrate = physics.SupplyCrate(world, space, (500 + xrand, yrand, 0), fueltexture, 0) xrand = random.randrange(0, 200) yrand = random.randrange(0, 25) yrand += 400 xrand += 25 yrand -= index * 10 foodcrate = physics.SupplyCrate(world, space, (500 + xrand, yrand, 0), foodtexture, 1) xrand = random.randrange(0, 200) yrand = random.randrange(0, 25) yrand += 400 xrand += 25 yrand -= index * 20 supplycrate = physics.SupplyCrate(world, space, (500 + xrand, yrand, 0), supplytexture, 2) supplycrates.append(fuelcrate) supplycrates.append(foodcrate) supplycrates.append(supplycrate) verts = [[0.0, 600.0, 0.0], [200.0, 500.0, 0.0], [400.0, 600.0, 0.0], [800.0, 500.0, 0.0], [1000.0, 600.0, 0.0], [1500.0, 550.0, 0.0], [2000.0, 600.0, 0.0], [2500.0, 400.0, 0.0], [3000.0, 600.0, 0.0]] faces = [[0, 1, 2], [2, 3, 4], [4, 5, 6], [6, 7, 8]] tris = ode.TriMeshData() tris.build(verts, faces) worldmesh = ode.GeomTriMesh(tris, space) gamestate = 0 basecampstarttime = 0 basecampendtime = 0 while 1: input(pygame.event.get()) keystate = pygame.key.get_pressed() if keystate[K_UP]: ship.thrustUp(1.0) if keystate[K_LEFT]: ship.rotate(-1.0) if keystate[K_RIGHT]: ship.rotate(1.0) if keystate[K_SPACE]: # find the nearest crate to pickup if ship.carryingitem == 0: index = 0 for curcrate in supplycrates: x, y, z = curcrate.mainbody.body.getPosition() x1, y1, z1 = ship.mainbody.body.getPosition() dx = x - x1 dy = y - y1 distance = math.sqrt(dx * dx + dy * dy) if distance < 100: ship.joinBody(world, space, curcrate.mainbody.body, curcrate.type, index) index += 1 else: ship.dropBody() if gamestate == 2: background.Bind() gl.DrawQuad((0, 0, 4024, 4024)) font.draw((300, 300), "OMG EVERYONE DIED\nGame Over\nYour score %d" % basecampendtime) if gamestate == 0: background.Bind() gl.DrawQuad((0, 0, 4024, 4024)) font.draw(( 20, 20 ), "INSTRUCTIONS:\n\nLEFTARROW: rotate ship left\nRIGHTARROW: rotate ship right\nUPARROW: thrust up\nSPACE: grapple supply crate\n\nThe point to the game is to keep the base camp\npopulation alive as long as possible. \nEach resource has a chain reaction on other resources\nwhich can in turn exponential kill off the population.\n\nGrap resource crates and fly them near the basecamp\nRefuel by landing on the fuel platform." ) glColor4f(1.0, 0.5, 0.5, 0.40) gl.DrawColorQuad((110, 520, 550, 32)) glColor4f(1.0, 1.0, 1.0, 1.0) font.draw((200, 520), "CLICK ANYWHERE TO START GAME") global mousedown if mousedown: basecampstarttime = pygame.time.get_ticks() gamestate = 1 if gamestate == 1: if basecamp.population < 1: gamestate = 2 basecampendtime = pygame.time.get_ticks() - basecampstarttime # basecamp takes resource off of your ship if within proximity x, y, z = ship.mainbody.body.getPosition() x1, y1, z1 = basecamp.mainbody.body.getPosition() dx = x1 - x dy = y1 - y distance = math.sqrt(dx * dx + dy * dy) if distance < 300 and ship.carryingitem == 1 and ship.carryindex > 0: if ship.carrytype == 0: basecamp.AddFuel() if ship.carrytype == 1: basecamp.AddFood() if ship.carrytype == 2: basecamp.AddSupply() ship.dropBody() del supplycrates[ship.carryindex] ship.carryingitem = 0 ship.carryindex = -1 # add fuel to ship only if really close to fuel station & landing gear not moving x, y, z = ship.mainbody.body.getPosition() x1, y1, z1 = fuelstation.mainbody.body.getPosition() dx = x1 - x dy = y1 - y distance = math.sqrt(dx * dx + dy * dy) leftgear = abs(ship.leftjoint.getPositionRate()) rightgear = abs(ship.rightjoint.getPositionRate()) gearforce = leftgear + rightgear if distance < 140 and gearforce < 0.5: ship.fuellevel += 0.01 if ship.fuellevel > 1.0: ship.fuellevel = 1.0 physics.RunSimulation(world, space, contactgroup, 1.0 / 60.0) # translate to the ship x, y, z = ship.mainbody.body.getPosition() glTranslatef(-x - 200, -y + 300, 0) background.Bind() gl.DrawQuad((0, 0, 4024, 4024)) #draw world for index in range(0, len(verts) - 1): glPushMatrix() x1 = verts[index][0] y1 = verts[index][1] x2 = verts[index + 1][0] y2 = verts[index + 1][1] gl.DrawLine((x1, y1), (x2, y2)) glPopMatrix() ship.draw() # draw basecamp basecamp.draw() fuelstation.draw() #draw supply crates for crate in supplycrates: crate.draw() # draw hud glPushMatrix() x, y, z = ship.mainbody.body.getPosition() glColor4f(1.0, 0.5, 0.5, 0.75) gl.DrawColorQuad((x - 375, y - 250, 300 * ship.fuellevel, 32)) glPopMatrix() fuel = basecamp.fuel food = basecamp.food supply = basecamp.supply pop = basecamp.population glPushMatrix() glColor4f(0.5, 1.0, 0.5, 0.75) gl.DrawColorQuad((x - 375, y + 150, fuel, 16)) glPopMatrix() glPushMatrix() gl.DrawColorQuad((x - 375, y + 190, food, 16)) glPopMatrix() glPushMatrix() gl.DrawColorQuad((x - 375, y + 230, supply, 16)) glPopMatrix() glPushMatrix() gl.DrawColorQuad((x - 375, y + 270, pop / 50, 16)) glPopMatrix() glColor4f(1.0, 1.0, 1.0, 1.0) font.draw((32, 435), "fuel %d" % fuel) font.draw((32, 435 + 38), "food %d" % food) font.draw((32, 435 + 38 * 2), "supply %d" % supply) font.draw((32, 435 + 38 * 3), "population %d" % pop) fuelleveltext = 'fuel level: %.0f' % (ship.fuellevel * 100) font.draw((32, 32), fuelleveltext) if ship.fuellevel < 0.3: fuelleveltext = 'LOW FUEL' font.draw((400, 300), fuelleveltext) font.draw((screenwidth - 200, 0), fpsstring) pygame.display.flip() fps += 1 clock.tick(60) if (pygame.time.get_ticks() - starttime > 1000): fpsdisplay = fps fpsstring = 'fps: %d' % fpsdisplay fps = 0 starttime = pygame.time.get_ticks()
world, world_time = ode.World(), reactor.seconds() world.setGravity((0, 0, 9.81)) body = ode.Body(world) M = ode.Mass() M.setSphere(800, 0.4) body.setMass(M) body.setPosition((3, 3, -3)) space = ode.HashSpace() body_geom = ode.GeomSphere(space, 0.4) body_geom.setBody(body) pool_mesh = threed.mesh_from_obj(open('scene/pool6_Scene.obj')) pool_geom = ode.GeomTriMesh(pool_mesh.ode_trimeshdata, space) def get_water_vel(pos): return (pos % v(0, 0, 1))*math.e**(-pos.mag()/3) imu_pos = v(0.43115992, 0.0, -0.00165058) def world_tick(): global world_time water_vel = get_water_vel(V(body.getPosition())) body.addForceAtRelPos((0, 0, -buoyancy_force(body.getPosition()[2], 0.4)), (0, 0, -.1)) body.addForce(-(1600 if body.getPosition()[2] >= 0 else 160) * (V(body.getLinearVel())-water_vel)) body.addForce([random.gauss(0, 1) for i in xrange(3)]) body.addTorque([random.gauss(0, 10) for i in xrange(3)])
def __init__(self, dt=1e-2, end_t=0, graphical=True, pave=False, plane=True, ground_grade=0.0, ground_axis=(0, 1, 0), publish_int=5, robot=None, robot_kwargs={}, start_paused = True, print_updates = False, render_objs = False, draw_contacts = False, draw_support = False, draw_COM = False, height_map = None, terrain_scales=(1, 1, 1)): """ if end_t is set to 0, sim will run indefinitely if graphical is set to true, graphical interface will be started plane turns on a smooth plane to walk on publish_int is the interval between data publishes in timesteps ground_grade slopes the ground around ground_axis by tan(ground_grade) degrees robot_kwargs get passed to the robot when it is instantiated """ self.sim_t = 0 self.dt = dt self.graphical = graphical self.plane = plane self.height_map = height_map self.publish_int = publish_int self.n_iterations = 0 self.real_t_laststep = 0 self.real_t_lastrender = 0 self.real_t_start = getSysTime() self.paused = start_paused self.print_updates = print_updates self.render_objs = render_objs self.draw_contacts = draw_contacts self.draw_support = draw_support self.draw_COM = draw_COM # ODE space object: handles collision detection self.space = ode.Space() # ODE world object: handles body dynamics self.world = ode.World() self.world.setGravity((0.0, 0.0, -9.81)) # Sponginess of the universe. These numbers are empirical self.world.setERP(0.8) self.world.setCFM(1E-6) # Joint group for the contact joints generated during collisions self.contactgroup = ode.JointGroup() self.contactlist = [] # FIXME: Poorly named lists. # bodies contains ode bodies that have mass and collision geometry self.bodies = [] self.geoms = [] self.graphics = [] # This is the publisher where we make data available self.publisher = Publisher(5055) self.publisher.start() # These are the bodies and geoms that are in the universe self.robot = robot(self, publisher=self.publisher, **robot_kwargs) # Start populating the publisher self.publisher.addToCatalog('time', self.getSimTime) self.publisher.addToCatalog('body.height', lambda: self.robot.getPosition()[2]) self.publisher.addToCatalog('body.distance', lambda: len3(self.robot.getPosition())) self.publisher.addToCatalog('body.velocity', lambda: len3(self.robot.getVelocity())) # Do graphics initialization if self.graphical: # initialize pygame pygame.init() # create the program window Screen = (800, 600) self.window = glLibWindow(Screen, caption="Robot Simulator") View3D = glLibView3D((0, 0, Screen[0], Screen[1]), 45) View3D.set_view() self.camera = glLibCamera(pos=(0, 10, 10), center=(0, 0, 0), upvec=(0, 0, 1)) glLibColorMaterial(True) glLibTexturing(True) glLibLighting(True) glLibNormalize(True) Sun = glLibLight([400, 200, 250], self.camera) Sun.enable() self.cam_pos = (0, 10, 10) # Create a plane for the robot to walk on if self.plane: g = self.createBoxGeom((1e2, 1e2, 1)) g.color = (0, 128, 0, 255) pos = (0, 0, -0.5) g.setPosition(pos) self.geoms.append(g) self.ground = g rot_matrix = calcRotMatrix(ground_axis, atan(ground_grade)) self.ground.setRotation(rot_matrix) if self.graphical: self.ground.texture = glLibTexture(TEXTURE_DIR + "dot.bmp") # Create a textured terrain from a heightmap image if self.height_map: # initialize pygame pygame.init() self.mesh = [] surf = pygame.image.load(os.path.join(terrain_dir, self.height_map)) dims = surf.get_size() self.ground_offset = (-dims[0] / 2.0, -dims[1] / 2.0, 0.0) # Create a list of lists. Map color to height. # Black = 0 height # Pure white = 1 meter height self.height_map = [[(terrain_scales[2] / (4 * 255.)) * sum(surf.get_at((x, y))) for y in range(dims[1])] for x in range(dims[0])] max_height = 0 # TODO: Verify height normalization correct in some other way for row in self.height_map: for h in row: max_height = max(max_height, h) print "Max height: %f" % max_height verts = [] faces = [] for x in range(dims[0]): for y in range(dims[1]): # verts.append((x+self.ground_offset[0],\ # y+self.ground_offset[1],\ # self.height_map[x][y]) ) verts.append((x + self.ground_offset[0], y + self.ground_offset[1], self.height_map[y][x])) def i_from_xy(x, y): return dims[1] * x + y for x in range(dims[0] - 1): for y in range(dims[1] - 1): faces.append((i_from_xy(x, y), i_from_xy(x + 1, y), i_from_xy(x, y + 1))) faces.append((i_from_xy(x + 1, y + 1), i_from_xy(x, y + 1), i_from_xy(x + 1, y))) tm = ode.TriMeshData() tm.build(verts, faces) self.ground_geom = ode.GeomTriMesh(tm, self.space) if self.graphical and self.height_map: self.ground_obj = glLibObjMap(self.height_map, normals=GLLIB_VERTEX_NORMALS, heightscalar=1.0)
def __init__(self, env, mesh): self.env=env self.mesh=mesh self.data=ode.TriMeshData() self.Rebuild() self.geom=ode.GeomTriMesh(self.data, env.space)
def _createGeom(self, geom, M): """Create an ODE geom object for the given cgkit geom. Create an ODE geom of the appropriate type (depening on \a geom) and apply the transformation \a M to it (currently, M may only contain a translation and a rotation, otherwise a warning is printed). The returned geom is not assigned to a Body yet. \param geom (\c GeomObject) cgkit geom object \param M (\c mat4) Transformation that's applied to the geom \return ODE geom """ # Create the raw geom object that's defined in the local # coordinate system L of the world object this geom belongs to. # Sphere geom? if isinstance(geom, SphereGeom): odegeom = ode.GeomSphere(None, geom.radius) # CCylinder geom? elif isinstance(geom, CCylinderGeom): odegeom = ode.GeomCCylinder(None, geom.radius, geom.length) # Box geom? elif isinstance(geom, BoxGeom): odegeom = ode.GeomBox(None, (geom.lx, geom.ly, geom.lz)) # TriMesh geom? elif isinstance(geom, TriMeshGeom): verts = [] for i in range(geom.verts.size()): verts.append(geom.verts.getValue(i)) # print "V",i,verts[i] faces = [] for i in range(geom.faces.size()): f = geom.faces.getValue(i) faces.append(f) # print "F",i,faces[i] ia, ib, ic = faces[i] a = verts[ia] b = verts[ib] c = verts[ic] sa = ((b - a).cross(c - a)).length() # if sa<0.0001: # print "*****KLEIN*****",sa tmd = ode.TriMeshData() tmd.build(verts, faces) odegeom = ode.GeomTriMesh(tmd, None) # Plane geom? elif isinstance(geom, PlaneGeom): L = obj.localTransform() n = L * vec3(0, 0, 1) - L * vec3(0) n = n.normalize() d = n * obj.pos odegeom = ode.GeomPlane(space, n, d) # Unknown geometry else: raise ValueError, 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s".' % geom.name # print 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s". Using bounding box instead.'%obj.name # bmin, bmax = obj.boundingBox().getBounds() # s = bmax-bmin # odegeom = ode.GeomBox(None, s) # pos,rot,scale = P.inverse().decompose() # odegeom.setPosition(pos + 0.5*(bmax+bmin)) # odegeomtransform = ode.GeomTransform(space) # odegeomtransform.setGeom(odegeom) # return odegeomtransform # Displace the geom by M pos, rot, scale = M.decompose() if scale != vec3(1, 1, 1): print 'WARNING: ODEDynamics: Scaled geometries are not supported' odegeom.setPosition(pos) # row major or column major? odegeom.setRotation(rot.getMat3().toList(rowmajor=True)) return odegeom