def construct(self): self.geom1 = ode.GeomSphere(objects.space, self.radius) self.geom2 = ode.GeomSphere(objects.space, self.radius) self.geom1.setPosition((self.pos[0], self.pos[1], 0)) self.geom2.setPosition((self.pos[0] + self.offset, self.pos[1], 0)) self.geom1.data = weakref.ref(self) self.geom2.data = weakref.ref(self)
def setupSimulation(N): global world, walls, space, geoms, objs world = ode.World() space = ode.Space() world.setERP(0.8) world.setCFM(1E-5) objs = [] planeConstraint = ode.Plane2DJoint(world) for i in range(N): ball = ode.Body(world) M = ode.Mass() M.setSphere(2500.0, 0.05) M.mass = 2 ball.setMass(M) bx = (np.random.rand() * 2 - 1) * 0.7 by = (np.random.rand() * 2 - 1) * 0.7 ball.setPosition((bx, by, 0)) geom = ode.GeomSphere(space, radius=0.2) geom.setBody(ball) geoms.append(geom) planeConstraint.attach(ball, ode.environment) objs.append(ball) walls = [] walls.append(ode.GeomPlane(space, (0, 1, 0), -1)) walls.append(ode.GeomPlane(space, (1, 0, 0), -1)) walls.append(ode.GeomPlane(space, (0, -1, 0), -1)) walls.append(ode.GeomPlane(space, (-1, 0, 0), -1))
def create_sphere(self, key, density, radius, pos): """Create a sphere and its corresponding geom. Arguments: @param key: number id to assign to the sphere @type key: int @param density: density of a sphere @type density: float @param radius: radius of a sphere @type radius: float @param pos: list of floats of the position for the sphere @type pos: [float,float,float] """ # Create body body = ode.Body(self.world) M = ode.Mass() M.setSphere(density, radius) body.setMass(M) # Set the position of the body. body.setPosition((pos)) # Set parameters for drawing the body body.shape = "sphere" body.radius = radius # Create a sphere geom for collision detection geom = ode.GeomSphere(self.space, radius) geom.setBody(body) # Append to the manager lists. self.bodies[key] = body self.geoms[key] = geom
def add_sphere(radius, density, position, quaternion=None, color=0x772277): """Create a sphere object, inlcuding visualization parameters. """ # Create rigid body and set position and rotation sphere = ode.Body(world) sphere.setPosition(position) if quaternion is not None: sphere.setQuaternion(quaternion) # Set the mass based on density mass = ode.Mass() mass.setSphere(density, radius) sphere.setMass(mass) # Create the collision geometry geom = ode.GeomSphere(space, radius) geom.setBody(sphere) geoms.append(geom) # Create the json primitive vis_prim = create_json_primitive("sphere", radius, color) json_object["primitives"].append(vis_prim) bodies.append(sphere) return sphere
def update(self, t): self.r1 = self.speed * (t - self.startT) self.r2 = self.speed * (t - self.endT) if self.geom1 is not None: self.geom1.setRadius(self.r1) else: self.geom1 = ode.GeomSphere(self.fieldSpace, self.r1) self.geom1.setPosition(self.center) self.geom1.parent = self if self.geom2 is not None: self.geom2.setRadius(self.r2) else: self.geom2 = ode.GeomSphere(self.fieldSpace, self.r2) self.geom2.setPosition(self.center) self.geom2.parent = self
def construct(self): self.body = ode.Body(world) M = ode.Mass() M.setSphereTotal(self.mass, self.rad) self.body.setMass(M) self.body.setPosition((self.x, self.y, 0)) self.geom = ode.GeomSphere(space, self.rad) self.geom.setBody(self.body) ODEThing.construct(self)
def _create_geom(self): # find vertex furthest from centroid max_rad = max([v.dot(v).item() for v in self.verts]) max_rad = math.sqrt(max_rad) # XXX Using sphere with largest vertex ray for broadphase for now self.geom = ode.GeomSphere(None, max_rad + self.eps.item()) self.geom.setPosition(torch.cat([self.pos, self.pos.new_zeros(1)])) self.geom.no_contact = set()
def _create_geom(self): # find vertex furthest from centroid max_rad = max([v.dot(v).data[0] for v in self.verts]) max_rad = math.sqrt(max_rad) # XXX Using sphere with largest vertex ray for broadphase for now self.geom = ode.GeomSphere(None, max_rad + self.eps.data[0]) self.geom.setPosition(torch.cat([self.pos.data, Tensor(1).zero_()])) self.geom.no_collision = set()
def _create_ball(world, ball_mass, ball_radius, space=None): mass = ode.Mass() mass.setSphereTotal(ball_mass, ball_radius) body = ode.Body(world) body.setMass(mass) body.shape = "sphere" body.boxsize = (2 * ball_radius, 2 * ball_radius, 2 * ball_radius) geom = ode.GeomSphere(space=space, radius=ball_radius) geom.setBody(body) return body, geom
def DefineSphere(self, Density, Radius): """Define this element as an ode Sphere.""" if self._hasGeom: self._geom = ode.GeomSphere(None, Radius) DisplayElement.SetDisplayObject(self, visual.sphere(radius=Radius)) self._mass = ode.Mass() self._mass.setSphere(Density, Radius) return self
def create_sphere(self, density, radius): body = ode.Body(self.world) M = ode.Mass() M.setSphere(density, radius) body.setMass(M) geom = ode.GeomSphere(self.space[0], radius=radius) geom.setBody(body) return body, geom
def create_sphere(self, key, density, radius, pos, active_surfaces={ 'x': 1, 'y': 1, 'z': 1, '-x': 1, '-y': 1, '-z': 1 }): """Create a sphere and its corresponding geom. Arguments: @param key: number id to assign to the sphere @type key: int @param density: density of a sphere @type density: float @param radius: radius of a sphere @type radius: float @param pos: list of floats of the position for the sphere @type pos: [float,float,float] """ # Auto label the joint key or not. key = len(self.bodies) if key == -1 else key # Create body body = ode.Body(self.world) M = ode.Mass() M.setSphere(density, radius) body.setMass(M) # Set the position of the body. body.setPosition((pos)) # Set parameters for drawing the body body.type = "sphere" body.shape = "sphere" body.radius = radius # Create a sphere geom for collision detection geom = ode.GeomSphere(self.space, radius) geom.setBody(body) # Append to the manager lists. self.bodies[key] = body self.geoms[key] = geom # Create surfaces if doing fluids if (self.fluid_dynamics): self.create_surfaces(key, 1., active_surfaces) return key
def DefineSphereTotal(self, TotalMass, Radius): """Define this element as an ode SphereTotal.""" if self._hasGeom: self._geom = ode.GeomSphere(None, Radius) DisplayElement.SetDisplayObject(self, visual.sphere(radius=Radius)) self._mass = ode.Mass() self._mass.setSphereTotal(TotalMass, Radius) self._mass.mass = TotalMass # Bug workaround? return self
def rand_target(self): pos = (np.random.rand(3,1)-0.5)*4 self.invalpos = (np.random.rand(3,1)-0.5) self.invalpos += np.sign(self.invalpos)* 0.2 self.invalpos += pos self.targetGeom = ode.GeomSphere(self.space, radius=0.1) self.targetGeom.setPosition((pos[0],pos[1],pos[2])) spikes = [3,4,5,7,9] temp= np.random.randint(3) self.targetParams = [0.12,spikes[temp]] self.invalidParams= [0.08,1,0.10*np.random.rand()+0.4] self.target = rendering.make_valid_target(self.targetParams[0],self.targetParams[1]) self.targetTrans = rendering.Transform() self.target.add_attr(self.targetTrans) self.invalid_target = rendering.make_invalid_target(self.invalidParams[0],self.invalidParams[1],self.invalidParams[2]) self.invalidTargetTrans = rendering.Transform() self.invalid_target.add_attr(self.invalidTargetTrans) self.intargetGeom = ode.GeomSphere(self.space, radius=0.1) self.intargetGeom.setPosition((self.invalpos[0],self.invalpos[1],self.invalpos[2])) return pos
def createSphere(self, sx, sz): sphere = ode.Body(self.world) mSphere = ode.Mass() mSphere.setSphere(PERSONDENSITY, SPHERERADIUS) sphere.setMass(mSphere) geomSphere = ode.GeomSphere(self.space, SPHERERADIUS) geomSphere.setBody(sphere) sphere.setPosition((sx, SPHERERADIUS, sz)) self.viz.addGeom(geomSphere) self.viz.GetProperty(sphere).SetColor(PERSONCOLOR) return sphere, geomSphere
def run_simulation(self): if self.trace: faulthandler.enable() sys.settrace(module.trace_func) # dynamics world world = ode.World() world.setGravity((0, -10, 0)) # heightmaps consider Y as up # collision space space = ode.SimpleSpace() # SimpleSpace is slow-ish # contact group contactgroup = ode.JointGroup() # mass M = ode.Mass() M.setSphere(2500, 1) M.mass = 1.0 # dynamic body body = ode.Body(world) body.setMass(M) body.setPosition((0, 260, 0)) body.setMovedCallback(self.body_callback) # collision geom geom = ode.GeomSphere(space, 1) geom.setBody(body) # heightmap h_data = ode.HeightfieldData() im = Image.open(sys.path[0] + "/tutorial_heightmap.png", "r") width, height = im.size pixel_values = list( im.getdata(0) ) # 0 is the index of the Red channel in RGB / RGBA images - returns a list of longs for some reason tho height_data = np.array(pixel_values) height_data = height_data.astype( np.ubyte) # convert array to unsigned bytes as ODE expects so height_data = np.ascontiguousarray( height_data ) # convert to a contiguous array so C code can read it and store data in variable to prevent garbage collection h_data.buildByte(height_data, False, width, height, width, height, 1, 0, 1, True) h_geom = ode.GeomHeightfield(data=h_data, space=space) # run simulation total_time = 0.0 dt = 0.04 while total_time < 4.5: print(total_time) space.collide((world, contactgroup), self.collision_callback) # collision detection world.quickStep(dt) # dynamics step contactgroup.empty() total_time += dt
def __init__(self, sim, density=1000, radius=1.0, label=None, **kw): body = ode.Body(sim.world) M = ode.Mass() M.setSphere(density, radius) self.size = (radius, radius, radius) # ? body.setMass(M) geom = ode.GeomSphere(sim.space, radius) geom.setBody(body) BodyItem.__init__(self, sim, body, geom, label, **kw)
def __init__(self,world,space,index,density,radius,grav): Object.__init__(self,world,index,grav) m = ode.Mass() m.setSphere(density,radius) self.body.setMass(m) # Create a geom for collision detection self.geom = ode.GeomSphere(space, radius=radius) self.geom.setBody(self.body) self.m = spheremass(density,radius)
def __init__(self): self.dt=.005 self.viewer = None self.viewerSize = 500 self.spaceSize = 6.4 self.resolution = self.viewerSize/self.spaceSize self.init_rod_template() self.seed() self.world = ode.World() #self.world.setGravity((0,-9.81,0)) self.world.setGravity((0,0,0)) self.body1 = ode.Body(self.world) self.body2 = ode.Body(self.world) self.body3 = ode.Body(self.world) self.body4 = ode.Body(self.world) self.create_link(self.body1,(0.5,0,0)) self.create_link(self.body2,(1.5,0,0)) self.create_link(self.body3,(2.5,0,0)) self.space = ode.Space() self.body4_col = ode.GeomSphere(self.space,radius=0.1) self.create_ee(self.body4,(3,0,0),self.body4_col) # Connect body1 with the static environment self.j1 = ode.HingeJoint(self.world) self.j1.attach(self.body1, ode.environment) self.j1.setAnchor( (0,0,0) ) self.j1.setAxis( (0,0,1) ) self.j1.setFeedback(1) # Connect body2 with body1 self.j2 = ode.HingeJoint(self.world) self.j2.attach(self.body1, self.body2) self.j2.setAnchor( (1,0,0) ) self.j2.setAxis( (0,0,-1) ) self.j2.setFeedback(1) #connect body3 with body2 self.j3 = ode.HingeJoint(self.world) self.j3.attach(self.body2, self.body3) self.j3.setAnchor( (2,0,0) ) self.j3.setAxis( (0,0,-1) ) self.j3.setFeedback(1) #connect end effector self.j4 = ode.FixedJoint(self.world) self.j4.attach(self.body3,self.body4) self.j4.setFixed() self.controlMode = "POS" self.targetPos = self.rand_target() self.targetTime = 0 self.P_gains = np.array([1000,1000,1000]) self.D_gains = np.array([70,50,20])
def __init__(self, u, par, level): Movable.__init__(self, u) RawGLObject.__init__(self, u) mass = ode.Mass() r = 0.05 density = 100 mass.setSphere( density, r ) #NB segment is a sphere mass -- so does not need to rotate. the bending is done by joints BETWEEN the masses self.body = ode.Body(self.u.world) self.body.setMass(mass) self.level = level x, y, z = par.getBodyPositionForChildSeg() seg = 0.4 if level == 1: x += 0.15 #65 z += 0.15 elif level == -1: x += 0.15 z -= 0.15 elif level > 1: x += seg z += seg elif level < -1: x += seg z -= seg #x+=x_offset #x=x+SEG_LENGTH #TODO fix properly #z=z+z_offset self.body.setPosition((x, y, z)) self.geom = ode.GeomSphere(u.segSpace, r) #SEPARATE SPACE! self.geom.setBody(self.body) self.sg.addChild(self.makeSceneGraph(r)) self.joint = ode.HingeJoint( self.u.world ) #easier to use horiz/vertical pairs of hinges rather than balls; as have getAngle methods self.joint.attach(par.body, self.body) self.joint.setAnchor(par.body.getPosition()) self.joint.setAxis((0, 1, 0)) if abs(level) == 1: #follicle has different properties self.k = 1.0 self.gamma = 0.5 else: self.k = 0.1 #TODO we really need two different classes of sim: one of rmacro navigation excersizes, and one for very fine detailed vibrtation sims. self.gamma = 0.05 self.par = par #needed for link graphics
def __init__(self, world, space, position, density, radius): self.body = ode.Body(world) self.body.setPosition(position) M = ode.Mass() M.setSphere(density, radius) self.body.setMass(M) self.radius = radius self._linear_damping_coeff *= radius self._rotational_damping_coeff *= radius # Create a Sphere geom for collision detection self.geom = ode.GeomSphere(space, radius) self.geom.setBody(self.body)
def create(self): self.body = ode.Body(self.world) mass = ode.Mass() mass.setCylinder(100, 2, 0.4, 1.8) mass.setSphere(100, 4) geom = ode.GeomCylinder(self.space, 0.4, 1.8) geom = ode.GeomSphere(self.space, 4) geom.setBody(self.body) x, y, z = self.position self.body.setRotation((1, 0, 0, 0, 0, -1, 0, 1, 0)) self.body.setPosition((x, y + 0.9, z)) self.viz.addGeom(geom) self.viz.GetProperty(self.body).SetColor(self.personColor) self.setLinearVelocity()
def DefineSphere(self, ElementKey, Body, Density, Radius): """Define this element as an ode Sphere.""" if self._hasGeom: self._geom = ode.GeomSphere(_bigSpace, Radius) DisplayElement.SetDisplayObject( self, ivisual.sphere(frame=Body._myFrame, radius=Radius)) self._mass = ode.Mass() self._mass.setSphere(Density, Radius) self.AddGDMElement(Body, ElementKey) return self
def __init__(self, id, universe, geometry="sphere", *args, **kwargs): self.id = id self.universe = universe self.body = ode.Body(universe.world) if geometry == "sphere": self.geom = ode.GeomSphere(universe.space) else: # TODO: Add other geometries raise ValueError("geometry must be 'sphere'") self.geom.setBody(self.body) all_entities[self.id] = self
def use_sphere(self): ''' Connect a ode.GeomSphere to the body The sphere radius is computed as the mean of the bounding box dimensions. As a consequence, if the TopoDS_Shape is a sphere, then the radius of the ode.GeomSphere will be the radius of the sphere ''' bbox = get_boundingbox(self._shape, EPSILON) xmin, ymin, zmin, xmax, ymax, zmax = bbox.Get() dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin r = ( dx + dy + dz ) / 6. # /3 gives the mean diameter, so divide by 2 for the radius self._collision_geometry = ode.GeomSphere(self._space, radius=r) self._collision_geometry.setBody(self)
def _create_sphere(self, space, density, radius): """Create a sphere body and its corresponding geom.""" # Create body and mass body = ode.Body(self.world) M = ode.Mass() M.setSphere(density, radius) body.setMass(M) body.name = None # Create a sphere geom for collision detection geom = ode.GeomSphere(space, radius) geom.setBody(body) geom.name = None return (body, geom)
def __init__(self, space, world, radius, mass, color=None): self._radius = radius self._color = color self._odemass = ode.Mass() self._odemass.setSphereTotal(0.00001, radius) self._odemass.adjust(mass) self._odebody = ode.Body(world) self._odebody.setMass(self._odemass) self._odegeom = ode.GeomSphere(space, radius) self._odegeom.setBody(self._odebody) # Observation self._neighbors = 2 self._obs_dim = 2 * self._neighbors + 2 # Acceleration of n nearest neighbors + Velocity of object
def DefineSphereTotal(self, ElementKey, Body, TotalMass, Radius): """Define this element as an ode SphereTotal.""" if self._hasGeom: self._geom = ode.GeomSphere(_bigSpace, Radius) DisplayElement.SetDisplayObject( self, ivisual.sphere(frame=Body._myFrame, radius=Radius)) self._mass = ode.Mass() self._mass.setSphereTotal(TotalMass, Radius) self._mass.mass = TotalMass # Bug workaround? self.AddGDMElement(Body, ElementKey) return self
def create_ball(world, space, density, radius): """create a ball body and its corresponding geom.""" #create the body (the physical volume to simulate) body = ode.Body(world) M = ode.Mass() M.setSphere(density, radius) body.setMass(M) # set parameters for drawing the body body.shape = "ball" body.radius = radius # create a sphere geom for collision detection geom = ode.GeomSphere(space, radius=body.radius) geom.setBody(body) return body
def create_sphere(world, space, density, radius): """Create a box body and its corresponding geom.""" # Create body body = ode.Body(world) M = ode.Mass() M.setSphereTotal(density, radius) body.setMass(M) # Set parameters for drawing the body body.shape = "sphere" body.radius = radius # Create a box geom for collision detection geom = ode.GeomSphere(space, radius=body.radius) geom.setBody(body) return body, geom