示例#1
0
def make_lower_leg(world):
    lower_leg_mass = Mass()
    lower_leg_mass.setBox(body_density, 30, 100, 5)
    lower_leg = Body(world)
    lower_leg.setMass(lower_leg_mass)
    servo = make_servo(world)
    fix(world, lower_leg, servo)
    return lower_leg
示例#2
0
 def __init__(self, world, pos, size, mass):
     self.world = world
     self.pos = pos
     # Create a body inside the world
     self.body = Body(world)
     M = Mass()
     M.setSphere(2500.0, size)
     M.mass = mass
     self.body.setMass(M)
     self.body.setPosition(pos)
     # draw body
     self.rbody = sphere(pos=pos, radius=size)
示例#3
0
def make_body(world):
    mass = Mass()
    mass.setBox(body_density, 240, 27, 150)
    body = Body(world)
    body.setMass(mass)
    fix(world, body, make_battery(world, (0, 27 / 2, -(150 / 2 - 35 / 2))))
    fix(world, body, make_battery(world, (0, 27 / 2, 150 / 2 - 35 / 2)))
    fix(world, body, make_servo(world, (240 / 2 - 30, 0, 150 / 2)))
    fix(world, body, make_servo(world, (240 / 2 - 30, 0, -150 / 2)))
    fix(world, body, make_servo(world, (-(240 / 2 - 30), 0, 150 / 2)))
    fix(world, body, make_servo(world, (-(240 / 2 - 30), 0, -150 / 2)))
    return body
示例#4
0
    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
示例#5
0
 def __init__(self, world, space, pos, size, mass, color=(0.7, 0.7, 0.7)):
     self.world = world
     self.pos = pos
     # Create a body inside the world
     self.body = Body(world)
     M = Mass()
     M.setSphere(2500, size)
     M.mass = mass
     self.body.setMass(M)
     self.body.setPosition(pos)
     if space:
         # setect colisions
         self.geom = GeomSphere(space, size)
         self.geom.setBody(self.body)
     # draw body
     self.rbody = sphere(pos=pos, radius=size, color=color)
示例#6
0
class Sphere(object):
    def __init__(self, world, pos, size, mass):
        self.world = world
        self.pos = pos
        # Create a body inside the world
        self.body = Body(world)
        M = Mass()
        M.setSphere(2500.0, size)
        M.mass = mass
        self.body.setMass(M)
        self.body.setPosition(pos)
        # draw body
        self.rbody = sphere(pos=pos, radius=size)

    def getPosition(self):
        return self.body.getPosition()

    def getLinearVel(self):
        return self.body.getLinearVel()

    def addForce(self, force):
        return self.body.addForce(force)

    def static(self):
        self.sj = BallJoint(self.world)
        self.sj.attach(self.body, environment)
        self.sj.setAnchor(self.pos)

    def joint(self, body):
        self.j = BallJoint(self.world)
        self.j.attach(self.body, body.body)
        self.j.setAnchor( body.pos )

    def draw(self):
        self.rbody.pos = self.getPosition()
示例#7
0
def make_battery(world, position=(0, 0, 0)):
    mass = Mass()
    mass.setBox(battery_density, 70, 25, 35)
    body = Body(world)
    body.setMass(mass)
    body.setPosition(position)
    return body
示例#8
0
def make_servo(world, position=(0, 0, 0)):
    mass = Mass()
    mass.setBox(servo_density, 40, 20, 35)
    body = Body(world)
    body.setMass(mass)
    body.setPosition(position)
    return body
示例#9
0
 def __init__(self, world, space, pos, size, mass, color=(0.7, 0.7, 0.7)):
     self.world = world
     self.pos = pos
     # Create a body inside the world
     self.body = Body(world)
     M = Mass()
     M.setSphere(2500, size)
     M.mass = mass
     self.body.setMass(M)
     self.body.setPosition(pos)
     if space:
         # setect colisions
         self.geom = GeomSphere(space, size)
         self.geom.setBody(self.body)
     # draw body
     self.rbody = sphere(pos=pos, radius=size, color=color)
示例#10
0
    def addToODE(self, simulator, x0, y0, z0):
        world = simulator.getWorld()
        morphology = self.morphology

        self.bodies = {}

        for part in morphology.getParts():
            self.bodies[part] = Body(world)

        for part in self.bodies:
            self.bodies[part].setMass(morphology.getWeight(part))

        #on commence par le bas !
        self.bodies['mollet_d'].setPosition(
            morphology.getLength('largeur') / 2, 0,
            morphology.getLength('mollet') / 2)
        self.bodies['mollet_g'].setPosition(
            -morphology.getLength('largeur') / 2, 0,
            morphology.getLength('mollet') / 2)

        self.bodies['cuisse_d'].setPosition(
            morphology.getLength('largeur') / 2, 0,
            morphology.getLength('mollet') +
            morphology.getLength('cuisse') / 2)
        self.bodies['cuisse_g'].setPosition(
            -morphology.getLength('largeur') / 2, 0,
            morphology.getLength('mollet') +
            morphology.getLength('cuisse') / 2)

        self.bodies['tronc_bassin_bas'].setPosition(
            0, 0,
            morphology.getLength('mollet') + morphology.getLength('cuisse') +
            morphology.getLength('tronc_bassin_bas') / 2)
        self.bodies['tronc_bassin_haut'].setPosition(
            0, 0,
            morphology.getLength('mollet') + morphology.getLength('cuisse') +
            morphology.getLength('tronc_bassin_bas') +
            morphology.getLength('tronc_bassin_haut') / 2)
示例#11
0
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
示例#12
0
def make_upper_leg(world):
    upper_leg_mass = Mass()
    upper_leg_mass.setBox(body_density, 30, 100, 5)
    upper_leg = Body(world)
    upper_leg.setMass(upper_leg_mass)
    return upper_leg
示例#13
0
 def __init__(self, scene):
     self.scene = scene
     self.body = Body(scene.world)
     self.scene.on_update(self.update)