def addBody(self, type, p1, p2, radius): p1 = add3(p1, self.offset) p2 = add3(p2, self.offset) length = dist3(p1, p2) body = ode.Body(self.world) m = ode.Mass() if type == 'horizontal': m.setCylinder(self.density, 3, radius, length) body.setMass(m) body.shape = "capsule" body.length = length body.radius = radius geom = ode.GeomCylinder(self.space, radius, length) geom.setBody(body) elif type == 'vertical': m.setBox(self.density, radius, radius, length) body.setMass(m) body.shape = "box" body.length = length body.radius = radius geom = ode.GeomBox(self.space, lengths=(radius, radius, length)) geom.setBody(body) elif type == 'base': m.setBox(BASE_DENSITY, radius, radius, length) body.setMass(m) body.shape = "box" body.length = length body.radius = radius geom = ode.GeomBox(self.space, lengths=(radius, radius, length)) geom.setBody(body) # define body rotation automatically from body axis. za = norm3(sub3(p2, p1)) if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7): xa = (1.0, 0.0, 0.0) else: xa = (0.0, 1.0, 0.0) ya = cross(za, xa) xa = norm3(cross(ya, za)) ya = cross(za, xa) rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2]) body.setPosition(mul3(add3(p1, p2), 0.5)) body.setRotation(rot) self.bodies.append(body) self.geoms.append(geom) self.totalMass += body.getMass().mass return body
def makePhysicsBody(self): physicsWorld = self.environment.world space = self.environment.space mainBody = ode.Body(physicsWorld) bodyMass = ode.Mass() totalMass = self.bodyMass+4*self.motorMass offset = self.armLength + float(self.bodyLength) / 2.0 #one arm bodyMass.setBoxTotal(totalMass/2.0, offset, self.bodyHeight, self.motorRadius) # next arm armMass = ode.Mass() armMass.setBoxTotal(totalMass/2.0, self.motorRadius, self.bodyHeight, offset) bodyMass.add(armMass) firstArmGeom = ode.GeomBox(space, (offset, self.bodyHeight, self.motorRadius)) firstArmGeom.setCategoryBits(1) firstArmGeom.setCollideBits(1) secondArmGeom = ode.GeomBox(space, (self.motorRadius, self.bodyHeight, offset)) secondArmGeom.setCategoryBits(1) secondArmGeom.setCollideBits(1) mainBody.setMass(bodyMass) firstArmGeom.setBody(mainBody) secondArmGeom.setBody(mainBody) self.geomList = [firstArmGeom, secondArmGeom] self.physicsBody = mainBody
def create_module(sim, parent_pos, lower_offset, upper_offset, rot): """ Spawns a CKBot module at the upper and lower body positions and specified rotation matrix. """ # Find the absolute positions of the module given the parent's position. lower_pos = (parent_pos[0] + lower_offset[0], parent_pos[1] + lower_offset[1], parent_pos[2] + lower_offset[2]) upper_pos = (parent_pos[0] + upper_offset[0], parent_pos[1] + upper_offset[1], parent_pos[2] + upper_offset[2]) hinge_pos = (0.5 * (lower_pos[0] + upper_pos[0]), 0.5 * (lower_pos[1] + upper_pos[1]), 0.5 * (lower_pos[2] + upper_pos[2])) # Create the lower piece of the module. lowerbody = ode.Body(sim.world) lowerjoint = ode.GeomBox(space=sim.space, lengths=(sim.cubesize, sim.cubesize * 0.5, sim.cubesize)) lowerjoint.setBody(lowerbody) lowerjoint.setPosition(lower_pos) lowerjoint.setRotation(rot) lowerM = ode.Mass() lowerM.setBox(sim.cubemass, sim.cubesize, sim.cubesize * 0.5, sim.cubesize) lowerbody.setMass(lowerM) # Create the upper piece of the module. upperbody = ode.Body(sim.world) upperjoint = ode.GeomBox(space=sim.space, lengths=(sim.cubesize, sim.cubesize * 0.5, sim.cubesize)) upperjoint.setBody(upperbody) upperjoint.setPosition(upper_pos) upperjoint.setRotation(rot) upperM = ode.Mass() upperM.setBox(sim.cubemass, sim.cubesize, sim.cubesize * 0.5, sim.cubesize) upperbody.setMass(upperM) # Create the hinge of the module. hinge = ode.HingeJoint(sim.world) hinge.attach(lowerbody, upperbody) hinge.setAnchor(hinge_pos) hinge.setAxis(rotate((1, 0, 0), rot)) hinge.setParam(ode.ParamLoStop, sim.hingeminangle) hinge.setParam(ode.ParamHiStop, sim.hingemaxangle) hinge.setParam(ode.ParamFMax, sim.hingemaxforce) # Append all these new pointers to the simulator class. sim.lowerbody.append(lowerbody) sim.lowerjoint.append(lowerjoint) sim.lowerM.append(lowerM) sim.upperbody.append(upperbody) sim.upperjoint.append(upperjoint) sim.upperM.append(upperM) sim.hinge.append(hinge)
def make_doors(self, space, half_width=0.1, offset=(0, 0)): geom1 = ode.GeomBox(space, (1 - 2 * half_width, 2 * half_width, half_width)) geom1.setPosition((self.end + 0.5, self.height, 0)) geom1.color = render.red x, y = offset geom2 = ode.GeomBox(space, (1 - 2 * half_width, 2 * half_width, half_width)) geom2.setPosition((self.end + 0.5 + x, self.height + y, 0)) geom2.color = render.red return [geom1, geom2]
def create(self): self.body = ode.Body(self.world) mass = ode.Mass() mass.setBox(ROADDENSITY, ROADLENGTH, ROADHEIGHT, self.size) self.body.setMass(mass) self.geom = ode.GeomBox(self.space, (ROADLENGTH, ROADHEIGHT, self.size)) self.geom.setBody(self.body) self.geom.setPosition((0, 0, self.zPos)) self.viz.addGeom(self.geom) self.viz.GetProperty(self.body).SetColor(ROADCOLOR) p = [] for n in range(int(self.numLanes)): p.append( (-100, self.yPos, self.zPos + (self.numLanes / 2 - n) * ROADSIZE, 1500, self.yPos, self.zPos + (self.numLanes / 2 - n) * ROADSIZE, 1)) p.append( (-100, self.yPos, self.zPos + self.numLanes / 2 * ROADSIZE, 1500, self.yPos, self.zPos + self.numLanes / 2 * ROADSIZE, 0)) p.append( (-100, self.yPos, self.zPos - self.numLanes / 2 * ROADSIZE, 1500, self.yPos, self.zPos - self.numLanes / 2 * ROADSIZE, 0)) if (self.numLanes % 2 == 0): if (self.direction): p.append((-100, self.yPos, self.zPos, 1500, self.yPos, self.zPos, 0)) else: p.append((-100, self.yPos, self.zPos, 1500, self.yPos, self.zPos, 0)) self.viz.drawLines(p)
def create_box(world, space, density, lx, ly, lz): """Create a box body and its corresponding geom.""" # Create body body = ode.Body(world) M = ode.Mass() M.setBox(density, lx, ly, lz) body.setMass(M) # print body.getMass() # Set parameters for drawing the body body.shape = "box" body.id = objcount + 1 body.attached = 0 body.attachedTo = [] body.facesActivated = [] body.facesActivatedTo = [] body.boxsize = (lx, ly, lz) body.delayCounter = 0 body.state = 'a' # Create a box geom for collision detection geom = ode.GeomBox(space, lengths=body.boxsize) geom.setBody(body) return body, geom
def _create_geom(self): self.geom = ode.GeomBox( None, torch.cat([self.dims + 2 * self.eps.item(), self.dims.new_ones(1)])) self.geom.setPosition(torch.cat([self.pos, self.pos.new_zeros(1)])) self.geom.no_contact = set()
def use_bounding_box(self): bbox = get_boundingbox(self._shape, EPSILON) xmin, ymin, zmin, xmax, ymax, zmax = bbox.Get() dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin self._collision_geometry = ode.GeomBox(self._space, lengths=(dx, dy, dz)) self._collision_geometry.setBody(self)
def self_create_box(self, density, lx, ly, lz): """Create a box body and its corresponding geom. Arguments: density - density of the given body lx : x dimension ly : y dimension lz : z dimension """ # Create body body = ode.Body(self.world) M = ode.Mass() M.setBox(density, lx, ly, lz) body.setMass(M) # Set parameters for drawing the body body.shape = "box" body.boxsize = (lx, ly, lz) # Create a box geom for collision detection geom = ode.GeomBox(self.space, lengths=body.boxsize) geom.setBody(body) return body, geom
def DefineBox(self, Density, SizeX, SizeY, SizeZ, colour, position=(0, 0, 0)): """Define this element as a ode Box.""" if self._hasGeom: self._geom = ode.GeomBox( _bigSpace, (SizeX, SizeY, SizeZ)) # BigSpace was None, problem with local body spaces DisplayElement.SetDisplayObject( self, visual.box(length=SizeX, height=SizeY, width=SizeZ, color=colour, pos=position)) self._mass = ode.Mass() self._mass.setBox(Density, SizeX, SizeY, SizeZ) return self
def addBox(self, lx, ly, lz, offset, mass=None): """ Add a box (cuboid) with x, y and z lengths given by lx, ly and lz. """ body = ode.Body(self.sim.world) # This is our own stupid shit body.color = (128, 128, 40, 255) m = ode.Mass() if mass is None: m.setBox(self.density, lx, ly, lz) else: m.setBoxTotal(mass, lx, ly, lz) body.setMass(m) # set parameters for drawing the body body.shape = "box" body.lx = lx body.ly = ly body.lz = lz # create a capsule geom for collision detection geom = ode.GeomBox(self.sim.space, (lx, ly, lz)) geom.setBody(body) # TODO: allow feeding in a starting rotation # For now all we need is position body.setPosition(add3(offset, self.offset)) self.bodies.append(body) self.geoms.append(geom) self.totalMass += body.getMass().mass return body
def shrink_box_avoiding_collision(self, box_index, collision_func): box = self.boxes[box_index] aabb = box.getAABB() end_height, start_height = aabb[-2:] height_half = (start_height - end_height) / 2.0 x_pos = box.position.x y_pos = box.position.y new_z = end_height box.setPosition((x_pos, y_pos, end_height - height_half)) loops_left = 12 upper_limit = start_height lower_limit = end_height if collision_func(box): # the cutter goes down to zero (end_height) - we can skip the rest loops_left = 0 while loops_left > 0: new_z = (upper_limit + lower_limit) / 2.0 box.setPosition((x_pos, y_pos, new_z - height_half)) if collision_func(box): upper_limit = new_z else: lower_limit = new_z loops_left -= 1 del self.boxes[box_index] # The height should never be zero - otherwise ODE will throw a # "bNormalizationResult" assertion. new_height = max(new_z - end_height, 0.1) z_pos = new_z - new_height / 2.0 new_box = ode.GeomBox(self.space, (aabb[1] - aabb[0], aabb[3] - aabb[2], new_height)) new_box.position = Point(x_pos, y_pos, z_pos) new_box.setBody(box.getBody()) new_box.setPosition((x_pos, y_pos, z_pos)) self.boxes.insert(box_index, new_box)
def create_box(self,key,dim,pos,terrain=False,density=10.,amp_adj=1.): """Create a box. Arguments: key: number id to assign to the box dim: list of l,w,h dimension for the box pos: list of x,y,z position for center of box terrain: flag indicating whether the box is terrain or not density: mass of the box amp_adj: factor for hydrodynamic calculations """ # Auto label the joint key or not. key = len(self.bodies) if key == -1 else key if not terrain: body, geom = self.self_create_box(density,dim[0],dim[1],dim[2]) body.setPosition((pos[0],pos[1],pos[2])) self.bodies[key] = body self.geoms[key] = geom if(self.fluid_dynamics): self.create_surfaces(key,amp_adj) else: geom = ode.GeomBox(self.space, lengths=(dim[0],dim[1],dim[2])) geom.setPosition((pos[0],pos[1],pos[2])) geom.static = True geom.shape = "box" geom.boxsize = (dim[0],dim[1],dim[2]) geom.color=[0.1,0.6,0.6,1.0] self.terrain_geoms[key] = geom return key
def addBody(self, p1, p2, radius): # Adds a capsule body between joint positions p1 and p2 and with given # radius to the ragdoll. p1 = add3(p1, self.offset) p2 = add3(p2, self.offset) # cylinder length not including endcaps, make capsules overlap by half # radius at joints cyllen = dist3(p1, p2) + radius """ body = ode.Body(self.world) m = ode.Mass() m.setCappedCylinder(self.density, 3, radius, cyllen) body.setMass(m) # set parameters for drawing the body body.shape = "capsule" body.length = cyllen body.radius = radius # create a capsule geom for collision detection geom = ode.GeomCCylinder(self.space, radius, cyllen) geom.setBody(body) """ # Create body body = ode.Body(self.world) m = ode.Mass() m.setBox(self.density, radius, radius, cyllen) body.setMass(m) # Set parameters for drawing the body body.shape = "rectangle" body.boxsize = (radius, radius, cyllen) # Create a box geom for collision detection geom = ode.GeomBox(self.space, lengths=body.boxsize) geom.setBody(body) # define body rotation automatically from body axis za = norm3(sub3(p2, p1)) if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7): xa = (1.0, 0.0, 0.0) else: xa = (0.0, 1.0, 0.0) ya = cross(za, xa) xa = norm3(cross(ya, za)) ya = cross(za, xa) rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2]) body.setPosition(mul3(add3(p1, p2), 0.5)) body.setRotation(rot) self.bodies.append(body) self.geoms.append(geom) self.totalMass += body.getMass().mass return body
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)
def construct(self): self.body = ode.Body(world) M = ode.Mass() M.setBoxTotal(self.mass, self.L, self.H, 1) self.body.setMass(M) self.body.setPosition((self.x, self.y, 0)) self.geom = ode.GeomBox(space, (self.L, self.H, 10)) self.geom.setBody(self.body) ODEThing.construct(self)
def _create_geom(self): self.geom = ode.GeomBox( None, torch.cat([ self.dims.data + 2 * self.eps.data[0], torch.ones(1).type_as(self.M.data) ])) self.geom.setPosition(torch.cat([self.pos.data, Tensor(1).zero_()])) self.geom.no_collision = set()
def __init__(self, world, space, position, density, lx, ly, lz): self.body = ode.Body(world) self.body.setPosition(position) M = ode.Mass() M.setBox(density, lx, ly, lz) self.body.setMass(M) self.geom = ode.GeomBox(space, lengths=(lx, ly, lz)) self.geom.setBody(self.body)
def create_box(self, density, lx, ly, lz): body = ode.Body(self.world) M = ode.Mass() M.setBox(density, lx, ly, lz) body.setMass(M) geom = ode.GeomBox(self.space[0], lengths=(lx, ly, lz)) geom.setBody(body) return body, geom
def __init__(self,density=1.0,bar_center=(0.0,0.0,0.0),bar_dim=(1.0,1.0,1.0),barycenters=None,he=1.0,cfl_target=0.9,dt_init=0.001): self.dt_init = dt_init self.he=he self.cfl_target=cfl_target self.world = ode.World() #self.world.setERP(0.8) #self.world.setCFM(1E-5) self.world.setGravity(g) self.g = np.array([g[0],g[1],g[2]]) self.space = ode.Space() eps_x = L[0]- 0.75*L[0] eps_y = L[1]- 0.75*L[1] #tank geometry #self.tankWalls = [ode.GeomPlane(self.space, (1,0,0) ,x_ll[0]+eps_x), # ode.GeomPlane(self.space, (-1,0,0),-(x_ll[0]+L[0]-eps_x)), #ode.GeomPlane(self.space, (0,1,0) ,x_ll[1]+eps_y), # ode.GeomPlane(self.space, (0,-1,0) ,-(x_ll[1]+L[1]-eps_y))] #mass/intertial tensor of rigid bar #eps_x = L[0]- 0.45*L[0] #eps_y = L[1]- 0.45*L[1] #self.tankWalls = [ode.GeomPlane(space, (1,0,0) ,x_ll[0]+eps_x), # ode.GeomPlane(space, (-1,0,0),-(x_ll[0]+L[0]-eps_x)), # ode.GeomPlane(space, (0,1,0) ,x_ll[1]+eps_y), # ode.GeomPlane(space, (0,-1,0) ,-(x_ll[1]+L[1]-eps_y))] #self.tank = ode.GeomBox(self.space,(0.45,0.45,0.3)) #self.tank.setPosition((0.5,0.5,0.6)) #self.contactgroup = ode.JointGroup() self.M = ode.Mass() self.totalMass = density*bar_dim[0]*bar_dim[1]*bar_dim[2] self.M.setBox(density,bar_dim[0],bar_dim[1],bar_dim[2]) #bar body self.body = ode.Body(self.world) self.body.setMass(self.M) self.body.setFiniteRotationMode(1) #bar geometry self.bar = ode.GeomBox(self.space,bar_dim) self.bar.setBody(self.body) self.bar.setPosition(bar_center) self.boxsize = (bar_dim[0],bar_dim[1],bar_dim[2]) #contact joints self.contactgroup = ode.JointGroup() self.last_position=bar_center self.position=bar_center self.last_velocity=(0.0,0.0,0.0) self.velocity=(0.0,0.0,0.0) self.h=(0.0,0.0,0.0) self.rotation = np.eye(3) self.last_rotation = np.eye(3) self.last_rotation_inv = np.eye(3) self.barycenters=barycenters self.init=True self.bar_dim = bar_dim self.last_F = np.zeros(3,'d') self.last_M = np.zeros(3,'d')
def DefineBoxTotal(self, TotalMass, SizeX, SizeY, SizeZ): """Define this element as an ode BoxTotal.""" if self._hasGeom: self._geom = ode.GeomBox(_bigSpace, (SizeX, SizeY, SizeZ)) DisplayElement.SetDisplayObject( self, visual.box(length=SizeX, height=SizeY, width=SizeZ)) self._mass = ode.Mass() self._mass.setBoxTotal(TotalMass, SizeX, SizeY, SizeZ) self._mass.mass = TotalMass # Bug workaround? return self
def __init__(self, world, space, node1, node2, radius=0.1, mass=1.0, rest_length=1, elasticity=0, color=(100., 0., 100.), fixed=False): self.nodes = [node1, node2] self.radius = radius self.mass = mass self.rest_length = rest_length self.elasticity = elasticity self.color = color self.joints = [] is_strut = elasticity == 0 self.body = ode.Body(world) positions = [node.get_position() for node in self.nodes] center = [(positions[0][i] + positions[1][i]) / 2 for i in range(len(positions[0]))] lengths = [(positions[1][i] - positions[0][i]) for i in range(len(positions[0]))] length = math.sqrt(dot(lengths, lengths)) self.update() print("lengths=", lengths) print("center=", center) if is_strut: geom_length = 0.9 * (length - (node1.radius + node2.radius)) self.geom = ode.GeomBox(space, lengths=[radius, geom_length, radius]) self.geom.setBody(self.body) for node in self.nodes: joint = ode.FixedJoint(world) joint.attach(self.body, node.body) joint.setFixed() self.joints.append(joint) if fixed: joint = ode.FixedJoint(world) joint.attach(self.body, ode.environment) joint.setFixed() self.joints.append(joint)
def __init__(self, space, world, size, mass, color=None): self._size = size self._color = color assert len(size) == 3 self._odebody = ode.Body(world) if mass: self._odemass = ode.Mass() self._odemass.setBox(1, *size) self._odemass.adjust(mass) self._odebody.setMass(self._odemass) self._odegeom = ode.GeomBox(space, size) self._odegeom.setBody(self._odebody)
def nuevo_objeto(mundo, espacio, densidad, lx, ly, lz): cubo = ode.Body(mundo) cubo.setPosition((0.0, 0, H / 2 + ZINI)) masa_cubo = ode.Mass() masa_cubo.setBoxTotal(densidad, lx, ly, lz) cubo.setMass(masa_cubo) cubo.sizebox = (lx, ly, lz) geometria = ode.GeomBox(espacio, lengths=cubo.sizebox) geometria.setBody(cubo) return cubo, geometria
def new_cube(xyz): """ Creates a new PyODE cude at position (x,y,z) """ body = ode.Body(world) M = ode.Mass() M.setBox(density, lx, ly, lz) body.setMass(M) body.shape = "box" body.boxsize = (lx, ly, lz) body.setPosition(xyz) geom = ode.GeomBox(space, lengths=body.boxsize) geom.setBody(body) geoms.append(geom) # avoids that geom gets trashed return body
def make_box(self, world, space): lx, ly, lz = self.parameters['boxlengths'] # Create body body = ode.Body(world) M = ode.Mass() M.setBox(self.parameters['density'], lx, ly, lz) body.setMass(M) # Create a box geom for collision detection geom = ode.GeomBox(space, lengths=(lx, ly, lz)) geom.setBody(body) return body, geom
def creando_cuerpo(mundo, espacio, densidad, lx, ly, lz): cuerpo = ode.Body(mundo) M = ode.Mass() M.setBox(densidad, lx, ly, lz) cuerpo.setMass(M) cuerpo.shape = "hexaedro" cuerpo.sizebox = (lx, ly, lz) geometria = ode.GeomBox(espacio, lengths=cuerpo.sizebox) geometria.setBody(cuerpo) return cuerpo, geometria
def _create_box(self, space, density, lx, ly, lz): """Create a box body and its corresponding geom.""" # Create body and mass body = ode.Body(self.world) M = ode.Mass() M.setBox(density, lx, ly, lz) body.setMass(M) body.name = None # Create a box geom for collision detection geom = ode.GeomBox(space, lengths=(lx, ly, lz)) geom.setBody(body) geom.name = None return (body, geom)
def add_box(i): import random i = i + 1 print "i:", i # Create a shape of random dimension rndX, rndY = random.uniform(-5, 5), random.uniform(-5, 5) _x, _y, _z = 5 + rndX, 10 + rndY, 20 new_box = BRepPrimAPI_MakeBox(_x, _y, _z).Shape() new_box_translated = translate_topods_from_vector( new_box, gp_Vec(0, 0, i * 40), False) d = dyn_context.register_shape(new_box_translated, enable_collision_detection=True, use_boundingbox=True) geom_box = ode.GeomBox(dyn_context._space, lengths=(_x, _y, _z)) geom_box.setBody(d) references.append(geom_box)
def create(self): self.road = ode.Body(self.world) mRoad = ode.Mass() mRoad.setBox(1, 3000, 0.01, self.size) self.road.setMass(mRoad) self.geomRoad = ode.GeomBox(self.space, (3000, 0.01, self.size)) self.geomRoad.setBody(self.road) self.geomRoad.setPosition((0, 0, 0)) self.viz.addGeom(self.geomRoad) self.reader = vtk.vtkJPGReader() self.reader.SetFileName(WHEEL_IMAGE) self.texture = vtk.vtkTexture() transform = vtk.vtkTransform() transform.Scale(1.0, 1.0, 1.0) self.texture.SetTransform(transform) self.viz.GetProperty(self.road).SetColor(ROADCOLOR)