def __init__(self, renderer=True, realtime=True, ip="127.0.0.1", port="21590", buf='16384'): """ initializes the virtual world, variables, the frame rate and the callback functions.""" print "ODEEnvironment -- based on Open Dynamics Engine." # initialize base class GraphicalEnvironment.__init__(self) if renderer: self.updateDone = True self.updateLock = threading.Lock() self.server = UDPServer(ip, port, buf) self.render=renderer self.realtime=realtime # initialize attributes self.resetAttributes() # initialize the textures dictionary self.textures = {} # initialize sensor and exclude list self.sensors = [] self.excludesensors = [] #initialize actuators list self.actuators = [] # A joint group for the contact joints that are generated whenever two bodies collide self.contactgroup = ode.JointGroup() self.dt = 0.01 self.FricMu = 8.0 self.stepsPerAction = 1 self.stepCounter = 0
def __init__(self, stype=STYPE.QTREE, gravity=None, sim=SIM.NORMAL): self.world=ode.World() if gravity is not None: self.world.setGravity(tuple(gravity)) self.space=self.SPACE_MAP[stype]() self.contactgroup=ode.JointGroup() self.sim=sim
def __init__(self, dt=(1 / 45.)): '''This section incorporates PyODE to define methods to create and draw three objects: a sphere, box, or cylinder. Some code borrowed from http://sourceforge.net/p/pyode/mailman/message/19674697/. ''' self.dt = dt self.entities = [] # Create a world object self.ode_world = ode.World() self.ode_world.setGravity(np.array([0, 0, Constants.g])) try: self.ode_world.setAngularDamping(0.2) except AttributeError: rospy.logerr( "SIM ERROR: You need to re-run the install script, or manually fix pyode" ) rospy.logerr("SIM ERROR: Killing simulation") exit(0) # self.ode_world.setLinearDamping(0.2) self.ode_world.setERP(0.8) self.ode_world.setCFM(1E-5) # Create a space object self.space = ode.Space() self.contact_group = ode.JointGroup() self.ode_world.step(self.dt)
def __init__(self): #CACHES self.physicals = [] #static and moving - for collision tests self.movings = [] #moving only self.robots = [] #things with controllers self.rawGLObjects = [] #things that need to call special graphics #ODE self.world = ode.World() self.world.setGravity((0, 0, 0)) #(0,-9.81,0) self.space = ode.Space() #for environment object geoms self.segSpace = ode.Space( ) #keep segs in sep space to reduce number of comparisons self.robotSpace = ode.Space() #for the robot body parts self.contactgroup = ode.JointGroup() #INVENTOR root = SoSeparator( ) #collide((u.world, u.contactgroup), contact_callback) #TODO replacewith custom collisions, segs vs whole world and robot vs whole world root.addChild(SoDirectionalLight()) self.myCamera = SoPerspectiveCamera() root.addChild(self.myCamera) myMaterial = SoMaterial() myMaterial.diffuseColor = (1.0, 0.0, 0.0) # Red root.addChild(myMaterial) env = SoSeparator() #the environent (ie not robots) root.addChild(env) self.root = root #CONTROLLER TIMER self.controllerTimer = 0
def __init__(self, max_simsecs, noise_sd, quick): log.debug('Sim.__init__(max_simsecs=%s)', max_simsecs) # create world, set default gravity, geoms, flat ground, spaces etc. assert type(max_simsecs) is float or type(max_simsecs) is int self.quick = quick self.total_time = 0.0 self.relax_time = 0 self.max_simsecs = float(max_simsecs) self.world = ode.World() if SOFT_WORLD: self.world.setCFM(1e-6) # (defaults to 1e-10) self.world.setERP(0.1) # (defaults to 0.2) self.world.setGravity((0, 0, -9.8)) self.space = ode.SimpleSpace() self.geoms = [] self.ground = ode.GeomPlane(self.space, (0, 0, 1), 0) self.geoms.append(self.ground) self.score = 0.0 self.finished = 0 self.siglog = None self.bpgs = [] self.contactGroup = ode.JointGroup() self.joints = [] self.noise_sd = noise_sd self.points = []
def start(cls, gravity=(0, -9.8, 0), erp=.8, cfm=1e-5): cls.world = ode.World() cls.world.setGravity(gravity) cls.world.setERP(erp) cls.world.setCFM(cfm) cls.space = ode.Space() cls.contactgroup = ode.JointGroup()
def __init__(self): ode.World.__init__(self) self._myScene = ivisual.display() # A joint group for the contact joints that are generated whenever # two bodies collide self._contactGroup = ode.JointGroup()
def run(self, nframes=None, top=1, bot=2, dt=None): world = self.world space = self.space contactgroup = ode.JointGroup() frame = 0 if dt is not None: self.dt = dt self.t = 0. running = True while running: line = self.serialize() yield line frame += 1 for i in range(top): space.collide((world, contactgroup), self.on_collision) world.step(self.dt / bot) self.t += self.dt / bot contactgroup.empty() for body in self.items: body.on_sim() #print(".", end="", flush=True, file=sys.stderr) if nframes is not None and frame >= nframes: break print("scale = %.4f" % (bot / (25. * self.dt * top)), file=sys.stderr)
def initialize(self): if not SubSystem.initialize(self): return False # Crating ode world self._world = ode.World() self._world.setGravity((0, -9.81, 0)) self._space = ode.Space() self._contactGroup = ode.JointGroup()
def __init__(self, gravity=-9.81): # Create a world object self.world = ode.World() self.space = ode.Space() self.contactgroup = ode.JointGroup() self.world.setGravity((0, gravity, 0)) #self.world.setCFM(0.000001) self.world.setERP(.99)
def __init__(self, col_callback, stepsize=0.005,log_data=False,output_path="",gravity=-9.81,fluid_dynamics=0,run_num=0,erp=0.5,cfm=1E-4): # Create a world object self.world = ode.World() self.world.setGravity((0,gravity,0) ) self.world.setERP(erp) self.world.setCFM(cfm) self.world.setContactMaxCorrectingVel(5.) # Run Number self.run_num = run_num # Create a space object self.space = ode.Space() # Create a plane geom which prevent the objects from falling forever self.floor = ode.GeomPlane(self.space, (0,1,0), 0) # A joint group for the contact joints that are generated whenever # two bodies collide self.contactgroup = ode.JointGroup() # A list of ODE Bodies self.bodies = {} # Maintain a data structure of the surfaces of each body if fluid dynamics are required. self.fluid_dynamics=fluid_dynamics if(fluid_dynamics): self.surfaces = {} # A list of geoms for the bodies self.geoms = {} # A dict of terrain geoms for the world. self.terrain_geoms = {} # A list of joints in the world. self.joints = {} # Register the collision callback self.col_callback = col_callback # Set the stepsize for the instance. self.stepsize = stepsize # Create a cushion for joint limits. self.joint_cushion = 0.#5. # Create a maximum velocity that a joint is able to move in one step. self.max_joint_vel = 36000. * self.stepsize*ANG_TO_RAD # Logging functionality self.log_data = log_data self.output_path = output_path if self.log_data: self.positions = [] self.quaternions = [] self.joint_angles = []
def reset(self): self._world = ode.World() self._space = ode.Space() self._obstacles = [] self._contacts = ode.JointGroup() self._drill = None self._drill_offset = None self._collision_detected = False self._dirty = True
async def game_loop(loop, universes): await load_ship_types() while running: joint_group = ode.JointGroup() start = loop.time() await step(universes, joint_group) await asyncio.sleep(max(0, 1 / TICK_RATE - (loop.time() - start)))
def initPhysics(self): import ode # Create a default world and a space self.world = ode.World() self.world.setGravity((0, -9.81, 0)) self.world.setERP(0.8) self.world.setCFM(1e-5) self.space = ode.Space() self.contactGroup = ode.JointGroup()
def make_world(grav=(0, -9.81, 0)): world = ode.World() world.setGravity(grav) world.setERP(0.8) world.setCFM(1E-5) space = ode.Space() bodies = [] geoms = [] contactgroup = ode.JointGroup() return world, space, bodies, geoms, contactgroup
def reset(self): # create an ODE world object global world world = ode.World() world.setGravity((0.0, -9.81, 0.0)) world.setERP(0.1) world.setCFM(1E-4) # create an ODE space object global space space = ode.Space() # create a plane geom to simulate a floor floor = ode.GeomPlane(space, (0, 1, 0), 0) # create a list to store any ODE bodies which are not part of the ragdoll (this # is needed to avoid Python garbage collecting these bodies) global bodies bodies = [] # create a joint group for the contact joints generated during collisions # between two bodies collide global contactgroup contactgroup = ode.JointGroup() # set the initial simulation loop parameters global Paused, lasttime, numiter, fps, dt, SloMo, stepsPerFrame fps = 60 dt = 1.0 / fps stepsPerFrame = 1 SloMo = 1 Paused = False lasttime = time.time() numiter = 0 # reward. global reward reward = 0 # create the pendulum global pendulum pendulum = pd.Pendulum(world, space, 500, (0.5, 0.0, 0.3)) print "total mass is %.1f kg (%.1f lbs)" % (pendulum.totalMass, pendulum.totalMass * 2.2) global TARGET_ANGLE TARGET_ANGLE = pendulum.hHinge.getAngle() # set GLUT callbacks #glutKeyboardFunc(self.onKey) #glutDisplayFunc(onDraw) #glutIdleFunc(onIdle) # enter the GLUT event loop self.SpinOnce()
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 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): self.world = ode.World() self.world.setGravity( Vector3(0,-9.81,0) ) self.space = ode.Space() self.boxes = {} self.contactgroup = ode.JointGroup() self.redis = redis.StrictRedis() self.redis_pubsub = redis.StrictRedis() self.channel = self.redis_pubsub.pubsub() self.channel.subscribe('phys') self.redis_fd = self.channel.connection._sock.fileno() self.floor = ode.GeomPlane(self.space, (0,1,0), 0)
def __init__(self): viz.EventClass.__init__(self) # Keep track of physnodes in here self.physNodes_phys = [] # This will be turned to TRUE when a collision has been detected self.collisionDetected = False # ODE initialization steps self.world = ode.World() print( 'physEnv.init(): FIX: Grav hardcoded at 9.8. Should accept gravity as a parameter, or include a function to change gravity' ) self.world.setGravity([0, -9.8, 0]) #self.world.setCFM(0.00001) #self.world.setERP(0.05) self.world.setCFM(0.00001) self.world.setERP(0.1) #self.world.setContactSurfaceLayer(0.001) ##bounce_vel is the minimum incoming velocity to cause a bounce # Collision space where geoms live and collisions are simulated # 0 for a 'simple' space (faster and less accurate), 1 for a hash space self.space = ode.Space(1) self.minBounceVel = .2 # min vel to cause a bounce #### A better description: ##Spaces are containers for geom objects that are the actual objects tested for collision. ##For the collision detection a Space is the same as the World for the dynamics simulation, and a geom object corresponds to a body object. ##For the pure dynamics simulation the actual shape of an object doesn't matter, you only have to know its mass properties. ##However, to do collision detection you need to know what an object actually looks like, and this is what's the difference between a body and a geom. # A joint group for the contact joints that are generated whenever two bodies collide self.jointGroup = ode.JointGroup() self.collisionList_idx = [] self.contactJoints_idx = [] self.contactObjects_idx = [] # A list of non-collision joints, such as fixed joints, etc self.joints_jIdx = [] ############################################################################################ ############################################################################################ ## Contact/collision functions vizact.onupdate(viz.PRIORITY_PHYSICS, self.stepPhysics)
def InitPhysics(): world = ode.World() world.setGravity((0, 60, 0)) world.setQuickStepNumIterations(100) world.setERP(0.1) space = ode.Space() #floor = ode.GeomPlane(space,(0,-1,0),-600) contactgroup = ode.JointGroup() return world, space, contactgroup
class PhysicsEngine(object): world = ode.World() space = ode.Space() contactgroup = ode.JointGroup() @classmethod def start(cls, gravity=(0, -9.8, 0), erp=.8, cfm=1e-5): cls.world = ode.World() cls.world.setGravity(gravity) cls.world.setERP(erp) cls.world.setCFM(cfm) cls.space = ode.Space() cls.contactgroup = ode.JointGroup() @classmethod def step(cls, step): cls.space.collide(None, cls._collidecallback) cls.world.step(step) cls.contactgroup.empty() @classmethod def _collidecallback(cls, args, geom1, geom2): go1 = geom1.gameobject go2 = geom2.gameobject go1.oncollision(go2) go2.oncollision(go1) for contact in ode.collide(geom1, geom2): contact.setBounce(0) contact.setMu(10000) j = ode.ContactJoint(cls.world, cls.contactgroup, contact) j.attach(geom1.getBody(), geom2.getBody()) @classmethod def createbody(cls): return ode.Body(cls.world) @classmethod def createmass(cls): return ode.Mass() @classmethod def creategeom(cls, geomtype, args): geomtype = "Geom" + geomtype try: return ode.__dict__[geomtype](cls.space, *args) except: raise PhysicsEngineError("Invalid Geom type: %s " "is not defined in `ode'" % geomtype)
def __init__(self): self.objectList = [] self.performance = 0 ode.World.__init__(self) self.coach = Coach() self.camera = Camera(0.0, 14.0, 19.5, -30.0, 0.0) self.objectList.append(self.camera) self.setGravity((0.0, -9.81, 0.0)) self.contactgroup = ode.JointGroup() self.space = ode.Space() self.accum = 0.0 self.time0 = 0 self.startTime = 0
def clear(self): ''' deletes all the ode geometry in the scene ''' if self._DISPLAY_INITIALIZED: self._display.EraseAll() for i in self._dynamic_shapes: if i._collision_geometry is None: continue else: if self._space.query(i._collision_geometry): self._space.remove(i._collision_geometry) self._dynamic_shapes = [] self._joints = [] self._space = ode.Space() self._contactgroup = ode.JointGroup()
def __init__(self, world, space, dt): ode_viz.ODE_Visualization.__init__(self, world, space, dt) self.contactgroup = ode.JointGroup() self.GetActiveCamera().SetPosition(0.725472557934, 0.888052344093, 5.504911860770) self.GetActiveCamera().SetFocalPoint(-0.0201045961788, 0.840176342681, -0.00458023275766) self.GetActiveCamera().SetViewUp(0.0287954448360, 0.999506133230, -0.0125822093364)
def __init__(self, dt=1. / 60, max_angular_speed=20): self.ode_world = ode.World() self.ode_world.setMaxAngularSpeed(max_angular_speed) self.ode_space = ode.QuadTreeSpace((0, 0, 0), (100, 100, 20), 10) self.ode_floor = ode.GeomPlane(self.ode_space, (0, 0, 1), 0) self.ode_contactgroup = ode.JointGroup() self.frame_no = 0 self.dt = dt self.elasticity = 0.1 self.friction = 2000 self.gravity = 0, 0, -9.81 self.cfm = 1e-6 self.erp = 0.7 self._bodies = {} self._joints = {}
def __init__(self, n_enemybots=12): self.n_robots = 12 self.n_enemybots = n_enemybots self.world = ode.World() self.world.setGravity((0, 0, -GRAVITY)) self.space = ode.HashSpace() self.space.enable() self.ground = ode.GeomPlane(self.space, (0, 0, 1), 0) self.contactgroup = ode.JointGroup() self.obj = Box(self.space, self.world, (LENGTH, WIDTH, HEIGHT), MASS) self.wall = [None for _ in range(nWall)] self.robot = [ SphereRobot(self.space, self.world, ROBOT_RADIUS, MASS) for _ in range(self.n_robots) ] self.joint = [None for _ in range(self.n_robots)] self.enemy_bot = [ SphereRobot(self.space, self.world, ROBOT_RADIUS, MASS, color=[0, 0, 1]) for _ in range(self.n_enemybots) ] self.seed() self.objv = deque(maxlen=3) [self.objv.append(np.zeros(2)) for _ in range(3)] self.result_force = np.zeros(2) self.objacc = np.zeros(2) self._is_static = True self.result_torque = 0 self.count = 0 self.drift_count = 0 self.sim_time = 0 self.fricdir = np.zeros(2) self.stage = 0 self.sum_err_f_mag_pid = 0 self.time_f_mag_pid = 0
def __init__(self): self.objectList = [] self.performance = 0 ode.World.__init__(self) self.setGravity((0.0, -9.81 / 2, 0.0)) self.space = ode.Space() self.contactgroup = ode.JointGroup() self.time0 = 0 #here are menus: mainmenu, ingame self.entryList = [ MainEntries(), InGameEntries(), RulesEntries(), AboutEntries() ] self.entryIndex = EntryIndex() self.font_aa = GLFont(r"DATA\\FNT\\bold_aa.glf") self.camera = Camera(0.0, 24.0, 0.0, -30.0, 0.0)
def createWorld(self, config): self.world = ode.World() self.space = ode.Space() self.contactgroup = ode.JointGroup() if config == None: config = ypc.WorldConfig() self.timeStep = config.timeStep self.world.setGravity(config.gravity) if config.planeHeight != None: self.plane = ode.GeomPlane(self.space, (0, 1, 0), config.planeHeight) self.world.setContactSurfaceLayer(config.ContactSurfaceLayer) self.world.setContactMaxCorrectingVel(config.ContactMaxCorrectingVel) self.world.setERP(config.ERP) self.world.setCFM(config.CFM)
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