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, world: ode.World = None):
"""
Args:
world: A py3ode :class:`World` object. If not provided, a new one will be
created with gravity ``(0,-9.81,0)``.
"""
if world is None:
world = ode.World()
world.setGravity((0, -9.81, 0))
self.world = world
self.maxStepSize = 0.05
self._lastUpdateSimTime = SimMan.now
SimMan.process(self._stateUpdater())
class odeRTB:
def __init__(self, gui):
self.gui = gui
self.vDHistory = []
self.bPHistory = []
self.th = TimeHelper()
self.w = World()
self.w.setGravity((0.0, 0.0, 0.0))
self.b = Body(self.w)
self.b.setPosition((0.0, 0.0, 0.0))
self.lt = self.th.getTimestamp(microsec=True)
self.lastVal = None
def plotAnimate(self, i):
xs = []
ys = []
zs = []
for v in self.bPHistory[:-10]:
xs.append(v[0])
ys.append(v[1])
zs.append(v[2])
if len(self.bPHistory) > 500:
self.bPHistory.pop(0)
self.vDHistory.pop(0)
self.ax1.clear()
r = np.array(xs)
s = np.array(ys)
t = np.array(zs)
self.ax1.scatter(r, s, zs, marker='o')
def startPltShow(self, a=''):
self.fig = plt.figure()
self.ax1 = self.fig.add_subplot(111, projection='3d')
ani = animation.FuncAnimation(self.fig, self.plotAnimate, interval=500)
plt.show()
iter = 0
def update(self, fromWho, val):
#print("ode.updateIt",fromWho)
self.iter += 1
if fromWho == 'accel':
if len(self.vDHistory) == 10:
_thread.start_new(self.startPltShow, ())
t = self.th.getTimestamp(microsec=True)
if self.lastVal == None:
self.lastVal = val
else:
vD = []
for k in range(len(val)):
vD.append(self.lastVal[k] - val[k])
self.vDHistory.append(vD)
#print(t-self.lt," mm/s^2:",vD)
#if (self.iter%100)==0:
# self.b.setLinearVel((0.0,0.0,.0))
# print("0.0.0")
bp = self.b.getPosition()
self.bPHistory.append(bp)
#print("b pos: {} {} {}".format(
# round(bp[0], 4),
# round(bp[1], 4),
# round(bp[2], 4)
# ))
avgFrom = 100
if len(self.vDHistory) % avgFrom == 0:
print("recall avg")
vcc = [0.0, 0.0, 0.0]
for ii in range(avgFrom - 1):
vcc[0] += self.vDHistory[ii][0]
vcc[1] += self.vDHistory[ii][1]
vcc[2] += self.vDHistory[ii][2]
vcc[0] /= float(avgFrom - 1.0)
vcc[1] /= float(avgFrom - 1.0)
vcc[2] /= float(avgFrom - 1.0)
self.va = vcc
vc = val
elif len(self.vDHistory) > avgFrom:
vc = [
val[0] - self.va[0], val[1] - self.va[1],
val[2] - self.va[2]
]
#print("va:",self.va)
else:
vc = val
# print("t",(t-self.lt)/1000000.0)
#print("vdh",len(self.vDHistory)," -",vc)
if len(self.vDHistory) > avgFrom + 10:
self.w.setGravity((
vD[0], #vc[0],#vD[0],
vD[1], #vc[1],#vD[1],
vD[2] #vc[2]#vD[2]
))
#self.w.step( (t-self.lt)/1000000.0 )
self.w.step(0.1)
self.lastVal = val
self.lt = t
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
rforce = -self.v1*(self.motors_force[3]+m_noise[3])*rotational_factor
self.motors[3].addForce(rforce)
@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)