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
