class Scene:
def __init__(self):
self.geometries = []
self.world = World()
self.world.setGravity((0, -9.8, 0))
self.world.setERP(0.4)
self.world.setCFM(1E-5)
self.space = Space()
self.contact_group = JointGroup()
self.update_callbacks = []
def add_geometry(self, geometry):
self.geometries.append(geometry)
def simulate(self, delta=0.04, duration=2.0, callback=None):
time = 0.0
collision_iterations = 4
while time < duration:
for i in range(collision_iterations):
# Detect collisions and create contact joints
self.space.collide((), self.near_callback)
# Simulate the world
self.world.step(delta / collision_iterations)
# Remove contact joints
self.contact_group.empty()
time += delta
for update_callback in self.update_callbacks:
update_callback(time)
if callback is not None:
callback(time)
def near_callback(self, _, geom1, geom2):
# Check if the objects do collide
contacts = collide(geom1, geom2)
# Create contact joints
for contact in contacts:
contact.setBounce(0.01)
contact.setMu(2000)
joint = ContactJoint(self.world, self.contact_group, contact)
joint.attach(geom1.getBody(), geom2.getBody())
def visualize(self, delta=0.04, duration=2.0, callback=None):
visualizer = Visualizer()
visualizer.create_window()
for geometry in self.geometries:
visualizer.add_geometry(geometry)
self.simulate(delta,
duration,
callback=VisualizationUpdater(visualizer, callback))
visualizer.destroy_window()
def on_update(self, callback):
self.update_callbacks.append(callback)
@property
def vel(self):
return self.center.getLinearVel()
######################################
# Simulation test
if __name__ == '__main__':
world = World()
world.setGravity((0, -9.81, 0))
world.setERP(0.8)
world.setCFM(1E-5)
space = Space()
contactgroup = JointGroup()
# Set environment
floor = GeomPlane(space, (0, 1, 0), -1)
box(pos=(0, -1, 0), width=2, height=0.01, length=2)
quad = Quad(world, space, pos=(0, -0.5, 0))
# set initial state of motors
# quad.motors_noise = (0, 0, 0, 0)
quad.motors_force = (1.92, 1.92, 1.92, 1.92)
