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 = []
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)
def __init__(self, Stable, wm, quad_pos=(0, 0, 0)): self.world = World() self.world.setGravity((0, -9.81, 0)) self.space = Space() self.contactgroup = JointGroup() self.wm = wm # draw floor self.floor = GeomPlane(self.space, (0, 1, 0), -1) box(pos=(0, -1, 0), width=2, height=0.01, length=2) # Draw Quad self.quad = Quad(self.world, self.space, pos=quad_pos) # Init Algorithm self.stable = Stable(self.quad) # stop autoscale of the camera scene.autoscale = False
class Simulator(object): def __init__(self, Stable, wm, quad_pos=(0, 0, 0)): self.world = World() self.world.setGravity((0, -9.81, 0)) self.space = Space() self.contactgroup = JointGroup() self.wm = wm # draw floor self.floor = GeomPlane(self.space, (0, 1, 0), -1) box(pos=(0, -1, 0), width=2, height=0.01, length=2) # Draw Quad self.quad = Quad(self.world, self.space, pos=quad_pos) # Init Algorithm self.stable = Stable(self.quad) # stop autoscale of the camera scene.autoscale = False # Initial noise #quad.motors[2].addForce((0.5, 1, 0)) # Collision callback def near_callback(self, args, geom1, geom2): """Callback function for the collide() method. This function checks if the given geoms do collide and creates contact joints if they do. """ # Check if the objects do collide contacts = collide(geom1, geom2) # Create contact joints world, contactgroup = args for c in contacts: c.setBounce(0.2) c.setMu(5000) j = ContactJoint(world, contactgroup, c) j.attach(geom1.getBody(), geom2.getBody()) def step(self): # Control Quad if controls are enabled res = self.wii_remote() # Detect collisions self.space.collide((self.world, self.contactgroup), self.near_callback) # run algorithm self.stable.step() self.world.step(self.dt) self.contactgroup.empty() # point camera to quad scene.center = self.quad.pos self.print_log() return res def wii_remote(self): if self.wm: if self.wm.state['buttons'] & cwiid.BTN_LEFT: self.stable.command_yaw = 1 if self.wm.state['buttons'] & cwiid.BTN_RIGHT: self.stable.command_yaw = -1 if self.wm.state['buttons'] & cwiid.BTN_1: self.stable.command_acceleration = 1 if self.wm.state['buttons'] & cwiid.BTN_2: self.stable.command_acceleration = -1 if self.wm.state['buttons'] & cwiid.BTN_A and self.wm.state['acc']: p = (wm.state['acc'][1] - 125.) / 125. r = (wm.state['acc'][0] - 125.) / 125. self.stable.command_pitch = -p self.stable.command_roll = r if self.wm.state['buttons'] & cwiid.BTN_B and self.wm.state['acc']: a = (wm.state['acc'][1] - 125.) / 125. self.stable.command_acceleration = a * 10 if self.wm.state['buttons'] & cwiid.BTN_HOME: self.wm.close() return True if not self.wm.state['buttons']: self.stable.command_pitch = 0 self.stable.command_roll = 0 self.stable.command_yaw = 0 self.stable.command_acceleration = 0 return False def print_log(self): print "%1.2fsec: pos=(%6.3f, %6.3f, %6.3f) vel=(%6.3f, %6.3f, %6.3f) pry=(%6.3f, %6.3f, %6.3f)" % \ (self.total_time, self.quad.pos[0], self.quad.pos[1], self.quad.pos[2], self.quad.vel[0], self.quad.vel[1], self.quad.vel[2], self.quad.pitch, self.quad.roll, self.quad.yaw) print "power=(%6.3f, %6.3f, %6.3f, %6.3f)" % \ (self.quad.motors_force[0], self.quad.motors_force[1], self.quad.motors_force[2], self.quad.motors_force[3]) def simulate(self): # Do the simulation... self.total_time = 0.0 self.dt = 0.04 self.stop = False while not self.stop: rate(self.dt*1000) self.step() self.total_time += self.dt
@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) # Collision callback def near_callback(args, geom1, geom2):