def __init__(self):
super().__init__()
self.model = Hopper()
self.__setOptimizationParams__(total_duration=0.4, epsilon=0.)
self.opti = ca.Opti()
self.var_dt = False
self.__setVariables__()
self.__setConstraints__()
self.__setCosts__()
def main():
theCompressor = Compresser() # create a Compressor object
theHopper = Hopper() # create a Hopper object
theHopper.fill_coal() # fill out the Hopper object with coal
# loop through and have the Compressor object take coal out of the Hopper
# and compress it.
for i in range(10):
print(theHopper)
theCompressor.get_coal(theHopper)
theCompressor.compress()
print(theCompressor)
def __init__(self, db, interface, related_interface, sniffer_name, enable_hopping,
use_24=True, use_pop5=False):
self.db = db
self.sniffer_name = sniffer_name
self.interface = interface
self.enable_hopping = enable_hopping
# Check interface existance
if not self._iface_exists(interface):
print "Exiting: Interface %s doesn't exist" % interface
sys.exit(1)
if related_interface and not self._iface_exists(related_interface):
print "Exiting: Related interface %s doesn't exist" % interface
sys.exit(1)
# Logging
header = 'SNIFF'
if sniffer_name:
header += '_' + sniffer_name
self.log = Log(self.db, use_stdout=True, header=header)
# Submodules
self.packet_parser = PacketParser(self.log)
self.hopper = Hopper(self.log, interface, related_interface)
ret = self.hopper.configure(use_24=use_24, use_pop5=use_pop5)
if ret is False:
sys.exit(1)
config.conf.sniff_promisc = 0
self.log.info("Promiscuous mode disabled")
self.watchdog = WatchDog(interval=20)
def __init__(self,
db,
interface,
related_interface,
sniffer_name,
enable_hopping,
use_24=True,
use_pop5=False):
self.db = db
self.sniffer_name = sniffer_name
self.interface = interface
self.enable_hopping = enable_hopping
# Check interface existance
if not self._iface_exists(interface):
print "Exiting: Interface %s doesn't exist" % interface
sys.exit(1)
if related_interface and not self._iface_exists(related_interface):
print "Exiting: Related interface %s doesn't exist" % interface
sys.exit(1)
# Logging
header = 'SNIFF'
if sniffer_name:
header += '_' + sniffer_name
self.log = Log(self.db, use_stdout=True, header=header)
# Submodules
self.packet_parser = PacketParser(self.log)
self.hopper = Hopper(self.log, interface, related_interface)
ret = self.hopper.configure(use_24=use_24, use_pop5=use_pop5)
if ret is False:
sys.exit(1)
config.conf.sniff_promisc = 0
self.log.info("Promiscuous mode disabled")
self.watchdog = WatchDog(interval=20)
def addControls(self):
#----- Setup/Manipulate Hopper -----
self.hopper = Hopper(self.render, self.bulletWorld, base)
self.accept('space', self.hopper.doJump)
self.accept('w', self.hopper.loopRunning)
self.accept('w-up', self.hopper.loopWalking)
self.accept('d', self.hopper.loopWalking)
self.accept('a', self.hopper.loopWalking)
self.accept('arrow_up', self.hopper.loopRunning)
self.accept('arrow_up-up', self.hopper.loopWalking)
self.accept('arrow_left', self.hopper.loopWalking)
self.accept('arrow_right', self.hopper.loopWalking)
self.accept('h', self.toggleHelp)
self.accept('l', self.toggleLight)
self.accept('b', self.toggleDebug)
self.accept('m', self.toggleMenu)
# local endpoint when running trained brain locally in docker container
url = "http://localhost:5000/v1/prediction"
response = requests.get(url, json=state)
action = response.json()
return action
if __name__ == '__main__':
logging.basicConfig()
log = logging.getLogger("hopper")
log.setLevel(level='DEBUG')
# we will use our environment (wrapper of OpenAI env)
hopper = Hopper(iteration_limit=1000)
# setting initial camera position
hopper.initialize_camera(distance=2, yaw=10, pitch=-20)
<<<<<<< HEAD
=======
# setting initial camera position
hopper.initialize_camera(distance=2, yaw=10, pitch=-20)
>>>>>>> a7a19c68ba9116d31ce4ce6aa41b78dcd515d250
# specify which agent you want to use,
# BonsaiAgent that uses trained Brain or
# RandomAgent that randomly selects next action
agent = BonsaiAgent()
import sys
sys.path.append('../libs')
import numpy as np
from hopper import Hopper
#raw_traj = np.array([[0,1,1.5,2,3,5,6,7,9,10], [0, 0.18559, 0.1813, 0.1921, 0.273, 0.567, 0.784, 0.674, 0.9373, 0.8389], [1.483, 2.485, 0.792, 1.13, 4.21, 11.78, 18.292, 8.11, 9.32, 12.31], [10.483, 21.485, 9.792, 21.13, 24.21, 31.78, 38.292, 28.11, 19.32, 112.31]])
test_hopper = Hopper()
class PlayHopper(ShowBase):
def __init__(self):
ShowBase.__init__(self)
#base.disableAllAudio()
#----- Setup Bullet World -----
self.debugNode = BulletDebugNode("Debug")
self.debugNode.showWireframe(True)
self.debugNP = self.render.attachNewNode(self.debugNode)
self.bulletWorld = BulletWorld()
self.bulletWorld.setGravity(Vec3(0, 0, -9.81))
self.bulletWorld.setDebugNode(self.debugNP.node())
#----- Setup Buttons ------
self.buttonMap = []
#----- State Variables -----
self.isHelping = False
self.isDebugging = False
self.isLit = False
self.isMenuShowing = False
self.amount = 0
self.isFatiguing = True
#----- Sounds ------
self.winSound = base.loader.loadSfx("sounds/jennyWin.m4a")
#----- Start Screen -----
self.ui = OnscreenInterface()
self.ui.createStartScreen()
self.startBtn = self.ui.startButton()
self.startBtn.configure(command = self.levelSelect, extraArgs = [self.setup])
self.buttonMap.append(self.startBtn)
#----- Level Select -----
self.level = 0
#(for developing purposes only), don't forget to uncomment line 152
#self.setup(2)
def levelSelect(self, command):
self.destroyButtons()
self.ui.destroyStartScreen()
self.ui.createLevelSelectScreen()
self.lev1 = self.ui.levelSelectButton(1, -0.6)
self.lev1.configure(command = command, extraArgs = [1])
self.lev1.resetFrameSize()
self.lev2 = self.ui.levelSelectButton(2, 0.55)
self.lev2.configure(command = command, extraArgs = [2])
self.lev2.resetFrameSize()
self.buttonMap.append(self.lev1)
self.buttonMap.append(self.lev2)
#----- Hopper Functions -----
def fatigue(self, task):
if self.hopper.getHealth() > 0:
self.hopper.lowerHealth(-8)
if self.hopper.getHealth() == 0:
self.isFatiguing = False
return task.done
else:
print self.hopper.getHealth()
self.isFatiguing = True
return task.again
def displayWallet(self, amount):
#---ITF: beautify this ----
self.wallet.setText("Wallet: "+str(amount))
#------ Level World Control -----
def setWorld(self, level):
if level == 1:
self.world = Level1World(self.render, self.loader, base, self.bulletWorld, self.hopper)
else:
self.world = Level2World(self.render, self.loader, base, self.bulletWorld, self.hopper)
def changeWorld(self, level):
self.destroyButtons()
self.ui.destroyLevelSelectScreen()
self.world.destroyWorld()
self.removeTasks()
self.stopAllSounds()
self.setWorld(level)
self.addMouse()
self.addTasks()
if len(self.world.enemies) != 0:
taskMgr.add(self.world.rayUpdate, "updatePicker")
self.destroyButtons()
self.resetHopper()
self.amount = 0
self.displayWallet(self.amount)
def addControls(self):
#----- Setup/Manipulate Hopper -----
self.hopper = Hopper(self.render, self.bulletWorld, base)
self.accept('space', self.hopper.doJump)
self.accept('w', self.hopper.loopRunning)
self.accept('w-up', self.hopper.loopWalking)
self.accept('d', self.hopper.loopWalking)
self.accept('a', self.hopper.loopWalking)
self.accept('arrow_up', self.hopper.loopRunning)
self.accept('arrow_up-up', self.hopper.loopWalking)
self.accept('arrow_left', self.hopper.loopWalking)
self.accept('arrow_right', self.hopper.loopWalking)
self.accept('h', self.toggleHelp)
self.accept('l', self.toggleLight)
self.accept('b', self.toggleDebug)
self.accept('m', self.toggleMenu)
def addMouse(self):
#----- Mouse Clicking -----
if len(self.world.enemies) != 0:
self.accept("mouseRayIntoEnemy", self.world.collideEventIn)
self.accept("mouseRayOutEnemy", self.world.collideEventOut)
self.accept("mouse1", self.world.mousePick, ['down'])
self.accept("mouse1-up", self.world.mousePick, ['up'])
print "Added mouse ray functions"
def setup(self, level):
self.destroyButtons()
self.ui.destroyLevelSelectScreen()
#----- Controls -----
self.addControls()
#----- Setup World -----
self.setWorld(level)
self.addMouse()
#----- Setup Camera -----
base.camera.reparentTo(self.hopper.hopperModel)
base.camera.setPos(0, 60, 50)#150.0)
base.camera.setH(180)
base.camera.lookAt(self.hopper.hopperModel)
#----- Tasks -----
#~ Permanent tasks
taskMgr.add(self.detectCollisionForGhosts, "detectEndCoinCollision", extraArgs = [self.world.endToken], appendTask = True, uponDeath = self.levelClear)
for spinner in self.world.spinners:
taskMgr.add(spinner.spin, "spinnerTask")
#~ Removable tasks
if len(self.world.enemies) != 0:
taskMgr.add(self.world.rayUpdate, "updatePicker")
taskMgr.add(self.world.update, "update")
taskMgr.add(self.world.simulateWater, "simulateWater", uponDeath = self.fail)
taskMgr.doMethodLater(2.5, self.fatigue, "fatigue", uponDeath = self.fail)
for berry in self.world.berries:
taskMgr.add(berry.spinBerry, "spinBerryTask")
taskMgr.add(self.detectCollisionForGhosts, "detectBerryCollision", extraArgs = [berry], appendTask = True, uponDeath = berry.collectBerry)
for coin in self.world.coins:
taskMgr.add(self.detectCollisionForGhosts, "detectCoinCollision", extraArgs = [coin], appendTask = True, uponDeath = coin.collectCoin)
for enemy in self.world.enemies:
taskMgr.add(self.detectCollision, "detectEnemyCollision", extraArgs = [enemy], appendTask = True)
for platform in self.world.spinningPlatforms:
taskMgr.add(platform.spinPlatform, "spinPlatformTask")
self.wallet = OnscreenText(text = "Wallet: "+str(self.amount), pos = (-1.1, -0.9), bg = (1, 1, 1, 1), align = TextNode.ACenter, mayChange = True)
#----- Item Functions -----
def detectCollisionForGhosts(self, item, task):
# contactTestPair returns a BulletContactResult object
contactResult = self.bulletWorld.contactTestPair(self.hopper.getNode(), item.ghostNode)
if len(contactResult.getContacts()) > 0:
print "Hopper is in contact with: ", item.ghostNode.getName()
if task.name == "detectCoinCollision":
item.setVolume(1)
self.amount += item.coinValue
self.displayWallet(self.amount)
elif task.name == "detectBerryCollision":
item.setVolume(1)
if item.berryValue > 0:
self.hopper.boostHealth(item.berryValue)
else:
self.hopper.lowerHealth(item.berryValue)
return task.done
else:
return task.cont
def detectCollision(self, cc, task):
# contactTestPair returns a BulletContactResult object
if cc.getHealth() != 0:
contactResult = self.bulletWorld.contactTestPair(self.hopper.getNode(), cc.getNode())
if len(contactResult.getContacts()) > 0:
if task.name == "detectEnemyCollision":
cc.setVolume(1)
self.hopper.lowerHealth(-0.3)
cc.attack()
return task.cont
else:
return task.cont
else:
return task.done
#----- Task Functions -----
def removeTasks(self):
taskMgr.remove("update")
taskMgr.remove("detectCoinCollision")
taskMgr.remove("detectBerryCollision")
taskMgr.remove("detectEnemyCollision")
taskMgr.remove("spinBerryTask")
taskMgr.remove("spinPlatformTask")
taskMgr.remove("spinnerTask")
taskMgr.remove("updatePicker")
def addTasks(self):
if self.isFatiguing == False:
taskMgr.doMethodLater(3, self.fatigue, "fatigue", uponDeath = self.fail)
for coin in self.world.coins:
print "Inside reset, adding coin task"
coin.setVolume(0)
taskMgr.add(self.detectCollisionForGhosts, "detectCoinCollision", extraArgs = [coin], appendTask = True, uponDeath = coin.collectCoin)
for berry in self.world.berries:
berry.setVolume(0)
taskMgr.add(berry.spinBerry, "spinBerryTask")
taskMgr.add(self.detectCollisionForGhosts, "detectBerryCollision", extraArgs = [berry], appendTask = True, uponDeath = berry.collectBerry)
for enemy in self.world.enemies:
enemy.setVolume(0)
taskMgr.add(self.detectCollision, "detectEnemyCollision", extraArgs = [enemy], appendTask = True)
for platform in self.world.spinningPlatforms:
taskMgr.add(platform.spinPlatform, "spinPlatformTask")
taskMgr.add(self.world.update, "update")
taskMgr.add(self.detectCollisionForGhosts, "detectEndCoinCollision", extraArgs = [self.world.endToken], appendTask = True, uponDeath = self.levelClear)
taskMgr.add(self.world.simulateWater, "simulateWater", uponDeath = self.fail)
for spinner in self.world.spinners:
taskMgr.add(spinner.spin, "spinnerTask")
if len(self.world.enemies) != 0:
taskMgr.add(self.world.rayUpdate, "updatePicker")
#----- Replay Functions -----
def reset(self):
print "Resetting level..."
for button in self.buttonMap:
button.destroy()
self.resetHopper()
self.removeTasks()
self.stopAllSounds()
self.world.backgroundMusic.play()
self.world.resetCoins()
self.world.resetBerries()
if len(self.world.enemies) != 0:
self.world.resetEnemies()
self.addTasks()
self.amount = 0
self.displayWallet(self.amount)
def levelClear(self, task):
#Hooray! Level 2 unlocked
#Menu options:
# - Main Menu
# - Quit
# - Play Again
# - Next Level
self.stopAllSounds()
self.winSound.play()
self.hopper.freeze = True
self.hopper.setHealth(-1)
self.q = DirectButton(text = ("Quit", "Quit", "Quit", "disabled"), scale = .08, pos = (0, 0, -0.2), command = self.quit)
self.q.resetFrameSize()
self.b = DirectButton(text = ("Restart Level", "Restart Level", "Restart Level", "disabled"), scale = .08, pos = (0, 0, -0.3) , command = self.reset)
self.b.resetFrameSize()
self.buttonMap.append(self.q)
self.buttonMap.append(self.b)
def resetHopper(self):
self.hopper.hopperNP.setPos(8, 10, 1)
self.hopper.hopperNP.setH(90)
self.hopper.resetHealth()
self.hopper.loopWalking()
self.hopper.freeze = False
def fail(self, task):
self.hopper.freeze = True
self.hopper.setHealth(-1)
print "Inside fail; hopper health:"+str(self.hopper.getHealth())
self.world.backgroundMusic.stop()
self.world.failSound.play()
self.c = DirectButton(text = ("Restart Level", "Restart Level", "Restart Level", "disabled"), scale = .08, pos = (0, 0, 0) , command = self.reset)
self.c.resetFrameSize()
self.buttonMap.append(self.c)
def quit(self):
sys.exit()
def stopAllSounds(self):
for berry in self.world.berries:
berry.stopSound()
for coin in self.world.coins:
coin.stopSound()
self.world.failSound.stop()
self.world.backgroundMusic.stop()
self.winSound.stop()
#----- User Input Functions -----
#ITF: add help menu to ui
def getHelp(self):
self.blackFrame = DirectFrame(frameColor=(0, 0, 0, 0.5), frameSize=(-3,3,-3,1),pos=(-1,1,1))
self.walk = self.addInstructions(0.3, "[W]: Forward")
self.turnLeft = self.addInstructions(0.2, "[A]: Turn Left")
self.turnRight = self.addInstructions(0.1, "[D]: Turn Right")
self.jump = self.addInstructions(0.0, "[Space]: Jump")
self.theHelp = self.addInstructions(-0.1, "[H]: Help")
self.lighting = self.addInstructions(-0.2, "[L]: Toggle Lighting")
self.debugging = self.addInstructions(-0.3, "[B]: Toggle Debugging")
def destroyHelp(self):
self.blackFrame.destroy()
self.walk.destroy()
self.turnLeft.destroy()
self.turnRight.destroy()
self.jump.destroy()
self.theHelp.destroy()
self.lighting.destroy()
self.debugging.destroy()
def addInstructions(self, pos, msg):
return OnscreenText(text = msg, style = 1, fg = (1, 1, 1, 1), pos = (0, pos), align= TextNode.ACenter, scale = .1)
def toggleHelp(self):
if self.isHelping == False:
self.getHelp()
self.isHelping = True
else:
self.destroyHelp()
self.isHelping = False
def toggleDebug(self):
if self.isDebugging == False:
self.debugNP.show()
self.isDebugging = True
else:
self.debugNP.hide()
self.isDebugging = False
def toggleLight(self):
if self.isLit == False:
self.hopper.freeze = True
self.world.addLight()
self.isLit = True
unfreezeSeq = Sequence(Wait(2.0), Func(self.hopper.unfreeze))
unfreezeSeq.start()
else:
self.world.destroyLight()
self.isLit = False
def toggleMenu(self):
if self.isMenuShowing == False:
self.isMenuShowing = True
self.ui.createMenu()
self.menuBtn = self.ui.menuButton("Level Select", 0)
self.menuBtn.configure(command = self.levelSelect, extraArgs = [self.changeWorld])
self.menuBtn.resetFrameSize()
self.buttonMap.append(self.menuBtn)
else:
self.isMenuShowing = False
self.ui.destroyMenu()
self.destroyButtons()
def destroyButtons(self):
for button in self.buttonMap:
button.destroy()
v0 = [0, 0]
w0 = 0
hipOffset = {'front': {'x': 0.5, 'z': -0.25}, 'hind': {'x': -0.5, 'z': -0.25}}
step_height = 0.1
platform1_start = -0.5
platform1_end = 0.3
platform1_height = 0*step_height
platform2_start = 0.7
platform2_end = 1.3
platform2_height = step_height
platform3_start = 1.7
platform3_end = 9.5
platform3_height = 2*step_height
matlab_hopper = eng.Hopper(legLength, hipOffset)
hop = Hopper(N, eng, matlab_hopper)
# hop.mdt_precision = int(ceil(-np.log2(desiredPrecision)))
hop.dtBounds = tuple(tf/N/sqrt(legLength/9.81)*np.array([0.1, 1.9]))
hop.rotationMax = np.pi/8
hop.nOrientationSectors = 1 #int(floor(np.pi/8/desiredPrecision))
print 'hop.nOrientationSectors = %d' % hop.nOrientationSectors
hop.velocityMax = 3
hop.positionMax = 7
hop.forceMax = 2
hop.addPlatform(platform1_start/legLength, platform1_end/legLength, platform1_height/legLength, 1)
hop.addPlatform(platform2_start/legLength, platform2_end/legLength, platform2_height/legLength, 1)
hop.addPlatform(platform3_start/legLength, platform3_end/legLength, platform3_height/legLength, 1)
hop.addFreeBlock(bottom=platform1_height/legLength, right=platform2_start/legLength)
hop.addFreeBlock(bottom=platform2_height/legLength, right=platform3_start/legLength)
hop.addFreeBlock(bottom=platform3_height/legLength)
hop.constructVisualizer()
class NLP:
def __init__(self):
super().__init__()
self.model = Hopper()
self.__setOptimizationParams__(total_duration=0.4, epsilon=0.)
self.opti = ca.Opti()
self.var_dt = False
self.__setVariables__()
self.__setConstraints__()
self.__setCosts__()
def __setOptimizationParams__(self, total_duration, epsilon):
self.T = total_duration
self.epsilon = epsilon
def __setVariables__(self):
if self.var_dt:
self.h = self.opti.variable(1)
self.opti.subject_to(self.opti.bounded(0.01, self.h,
self.model.dt))
# self.opti.subject_to(ca.sum1(self.h) == self.T / self.N)
self.opti.set_initial(self.h, 0.05)
else:
self.h = 0.05
self.N = int(self.T / self.h)
# self.states = []
# self.dstates = []
# self.actions = []
# self.forces, self.gammas = [], []
#
# for i in range(self.N):
# self.states.append(self.opti.variable(self.model.n_generalized, 1))
# self.dstates.append(self.opti.variable(self.model.n_generalized, 1))
#
# self.actions.append(self.opti.variable(self.model.dof, 1))
# self.forces.append(self.opti.variable(self.model.dims + 1, self.model.n_contact))
# self.gammas.append(self.opti.variable(1))
self.states = self.opti.variable(self.model.n_generalized, self.N)
self.dstates = self.opti.variable(self.model.n_generalized, self.N)
self.actions = self.opti.variable(self.model.dof, self.N)
self.forces, self.gammas = self.opti.variable(
3, self.N), self.opti.variable(1, self.N)
# self.start_state = ca.DM([0, (self.model.length[1, 0] * np.cos(np.pi/6) +
# self.model.length[2, 0]) * np.cos(np.pi/6), -np.pi/6, np.pi/6])
#
# self.end_state = ca.DM([2, (self.model.length[1, 0] * np.cos(np.pi/6) +
# self.model.length[2, 0]) * np.cos(np.pi/6), -np.pi/6, np.pi/6])
# self.start_state = ca.DM([0, (self.model.length[1, 0] +
# self.model.length[2, 0]), 0, 0])
#
# self.end_state = ca.DM([2, (self.model.length[1, 0] +
# self.model.length[2, 0]), 0, 0])
self.start_state = ca.DM(
[0, (self.model.length[1, 0] + self.model.length[2, 0]) * 0.7])
self.end_state = ca.DM(
[4, (self.model.length[1, 0] + self.model.length[2, 0]) * 0.7])
def __setConstraints__(self):
# self.opti.subject_to(self.states[:, 0] == self.start_state)
# self.opti.subject_to(self.states[:, -1] == self.end_state)
self.opti.subject_to(self.states[0:2, 0] == self.start_state)
self.opti.subject_to(self.states[0:2, -1] == self.end_state)
self.opti.subject_to(self.dstates[:,
0] == [0] * self.model.n_generalized)
self.opti.subject_to(self.dstates[:, -1] == [0] *
self.model.n_generalized)
for k in range(self.N - 1):
q_1, dq_1 = self.states[:, k], self.dstates[:, k]
q_2, dq_2 = self.states[:, k + 1], self.dstates[:, k + 1]
u_1, u_2 = self.actions[:, k], self.actions[:, k + 1]
lam_1, lam_2 = self.forces[:, k], self.forces[:, k + 1]
k_1 = self.model.kinematics(q=q_1)
k_2 = self.model.kinematics(q=q_2)
self.opti.subject_to(
k_1['p'][1, 1] >= self.model.terrain.heightMap(k_1['p'][0, 1]))
self.opti.subject_to(k_1['p'][1, 0] >= k_1['p'][1, 1])
self.opti.subject_to(k_1['p'][0, 1] <= q_1[0])
# if k == self.N/2:
# self.opti.subject_to(k_1['p'][1, 1] >= self.model.length[1, 0])
if k == 0:
self.opti.subject_to(
k_1['p'][1,
1] == self.model.terrain.heightMap(k_1['p'][0,
1]))
if k == self.N - 2:
self.opti.subject_to(
k_2['p'][1,
1] == self.model.terrain.heightMap(k_2['p'][0,
1]))
# self.opti.subject_to(k_1['p'][0, 1] <= q_1[0])
f_1 = self.model.dynamics(q=q_1, dq=dq_1, lam=lam_1)
f_2 = self.model.dynamics(q=q_2, dq=dq_2, lam=lam_2)
h = self.h
self.opti.subject_to(q_1 - q_2 + h * dq_2 == 0)
self.opti.subject_to(f_2['H'] @ (dq_2 - dq_1) - h *
(f_2['C'] @ dq_2 + f_2['G'] - f_2['B'] @ u_2 -
f_2['B_J_C'].T @ f_2['B_lam']) == 0)
# friction constraints
# self.opti.subject_to(lam_1 >= 0)
# self.opti.subject_to(f_1['phi'] >= 0)
# self.opti.subject_to(f_1['phi'].T @ lam_1 == 0)
lam_1_z, lam_1_xp, lam_1_xm = self.forces[2, k], self.forces[
0, k], self.forces[1, k]
gam_1 = self.gammas[k]
psi_1 = f_1['psi']
self.opti.subject_to(f_1['phi'] >= 0)
self.opti.subject_to(
[lam_1_z >= 0, lam_1_xp >= 0, lam_1_xm >= 0, gam_1 >= 0])
self.opti.subject_to(
self.model.terrain.mu * lam_1_z - lam_1_xp - lam_1_xm >= 0)
self.opti.subject_to(gam_1 + psi_1 >= 0)
self.opti.subject_to(gam_1 - psi_1 >= 0)
# self.opti.subject_to(f_1['phi'].T @ lam_1_z <= self.epsilon)
self.opti.subject_to(f_1['phi'].T @ lam_1_z == 0)
self.opti.subject_to((self.model.terrain.mu * lam_1_z - lam_1_xp -
lam_1_xm).T @ gam_1 == 0)
# self.opti.subject_to((self.model.terrain.mu * lam_1_z - lam_1_xp - lam_1_xm).T @ lam_1_z >= -self.epsilon)
# self.opti.subject_to((gam_1 + psi_1).T @ lam_1_xp <= self.epsilon)
self.opti.subject_to((gam_1 + psi_1).T @ lam_1_xp == 0)
# self.opti.subject_to((gam_1 - psi_1).T @ lam_1_xm <= self.epsilon)
self.opti.subject_to((gam_1 - psi_1).T @ lam_1_xm == 0)
########################
self.opti.subject_to(
self.opti.bounded(
self.start_state[0].full() * ca.MX.ones(1, self.N),
self.states[0, :],
self.end_state[0].full() * ca.MX.ones(1, self.N)))
# self.opti.subject_to(self.opti.bounded((-np.pi/2.5)*ca.MX.ones(1, self.N),
# self.states[2, :],
# ca.MX.zeros(1, self.N)))
# self.opti.subject_to(self.opti.bounded((np.pi/7)*ca.MX.ones(1, self.N),
# self.states[3, :],
# (2*np.pi/3)*ca.MX.ones(1, self.N)))
self.opti.subject_to(
self.opti.bounded(-15 * ca.MX.ones(2, self.N), self.actions,
15 * ca.MX.ones(2, self.N)))
def __setCosts__(self):
Q = ca.diag(ca.DM([0.1, 0.1, 10, 10]))
R = ca.diag(ca.DM([100, 100]))
cost = 0
for k in range(self.N):
q, dq = self.states[:, k], self.dstates[:, k]
u = self.actions[:, k]
# cost += dq.T @ Q @ dq + u.T @ R @ u
cost += (dq[2] * u[0])**2 + (dq[3] * u[1])**2
self.opti.minimize(cost)
def __solve__(self):
p_opts = {"expand": True}
s_opts = {"max_iter": 3000}
self.opti.solver("ipopt", p_opts, s_opts)
try:
self.solution = self.opti.solve_limited()
self.debug = False
except:
self.debug = True
def __interpolate__(self):
self.x_B = []
self.dx_B = []
self.z_B = []
self.dz_B = []
self.q_H = []
self.dq_H = []
self.q_K = []
self.dq_K = []
self.u_K = []
self.u_H = []
if self.debug:
if self.var_dt:
self.dt = self.opti.debug.value(self.h)
else:
self.dt = self.h
for i in range(self.N):
self.x_B.append(self.opti.debug.value(self.states[0, i]))
self.z_B.append(self.opti.debug.value(self.states[1, i]))
self.q_H.append(self.opti.debug.value(self.states[2, i]))
self.q_K.append(self.opti.debug.value(self.states[3, i]))
self.dx_B.append(self.opti.debug.value(self.dstates[0, i]))
self.dz_B.append(self.opti.debug.value(self.dstates[1, i]))
self.dq_H.append(self.opti.debug.value(self.dstates[2, i]))
self.dq_K.append(self.opti.debug.value(self.dstates[3, i]))
self.u_H.append(self.opti.debug.value(self.actions[0, i]))
self.u_K.append(self.opti.debug.value(self.actions[1, i]))
else:
if self.var_dt:
self.dt = self.solution.value(self.h)
else:
self.dt = self.h
for i in range(self.N):
self.x_B.append(self.solution.value(self.states[0, i]))
self.z_B.append(self.solution.value(self.states[1, i]))
self.q_H.append(self.solution.value(self.states[2, i]))
self.q_K.append(self.solution.value(self.states[3, i]))
self.dx_B.append(self.solution.value(self.dstates[0, i]))
self.dz_B.append(self.solution.value(self.dstates[1, i]))
self.dq_H.append(self.solution.value(self.dstates[2, i]))
self.dq_K.append(self.solution.value(self.dstates[3, i]))
self.u_H.append(self.solution.value(self.actions[0, i]))
self.u_K.append(self.solution.value(self.actions[1, i]))
self.k = self.model.dt / 8
self.t = np.linspace(0, self.N * self.dt,
int(self.N * self.dt / self.k))
print(self.dt)
self.t_points = np.linspace(0, self.N * self.dt, self.N)
x_B_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.x_B)
self.x_B_spline = x_B_spline_function(self.t)
z_B_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.z_B)
self.z_B_spline = z_B_spline_function(self.t)
q_H_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.q_H)
self.q_H_spline = q_H_spline_function(self.t)
q_K_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.q_K)
self.q_K_spline = q_K_spline_function(self.t)
dx_B_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dx_B)
self.dx_B_spline = dx_B_spline_function(self.t)
dz_B_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dz_B)
self.dz_B_spline = dz_B_spline_function(self.t)
dq_H_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dq_H)
self.dq_H_spline = dq_H_spline_function(self.t)
dq_K_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dq_K)
self.dq_K_spline = dq_K_spline_function(self.t)
self.__plot__()
def __plot__(self):
fig = plt.figure()
fig.tight_layout()
ax1 = fig.add_subplot(311)
ax1.grid()
ax1.plot(self.t_points, self.x_B, 'o', label='xB')
ax1.plot(self.t_points, self.z_B, 'o', label='zB')
# ax1.plot(self.t_points, self.q_H, 'o', label='qH')
# ax1.plot(self.t_points, self.q_K, 'o', label='qK')
ax1.plot(self.t, self.x_B_spline, '-', color='black')
ax1.plot(self.t, self.z_B_spline, '-', color='black')
# ax1.plot(self.t, self.q_H_spline, '-', color='black')
# ax1.plot(self.t, self.q_K_spline, '-', color='black')
ax1.legend()
ax2 = fig.add_subplot(312)
ax2.grid()
ax2.plot(self.t_points, self.dx_B, 'o', label='dxB')
ax2.plot(self.t_points, self.dz_B, 'o', label='dzB')
# ax2.plot(self.t_points, self.dq_H, 'o', label='dqH')
# ax2.plot(self.t_points, self.dq_K, 'o', label='dqK')
ax2.plot(self.t, self.dx_B_spline, '-', color='black')
ax2.plot(self.t, self.dz_B_spline, '-', color='black')
# ax2.plot(self.t, self.dq_H_spline, '-', color='black')
# ax2.plot(self.t, self.dq_K_spline, '-', color='black')
ax2.legend()
ax3 = fig.add_subplot(313)
ax3.grid()
ax3.plot(self.t_points, self.u_H, '-', label='u1')
ax3.plot(self.t_points, self.u_K, '-', label='u2')
ax3.legend()
plt.show()
self.model.visualize(self.x_B_spline.full(), self.z_B_spline.full(),
self.q_H_spline.full(), self.q_K_spline.full(),
self.t, self.k)
from math import sqrt
from hopper import Hopper
from hopperUtil import *
desiredPrecision = 2
N = 25
tf = 2*1.6
legLength = 0.16
r0 = [0, legLength/2]
rf = [1.0, legLength]
v0 = [0, 0]
w0 = 0
hipOffset = {'front': {'x': 0.5, 'z': -0.25}, 'hind': {'x': -0.5, 'z': -0.25}}
matlab_hopper = eng.Hopper(legLength, hipOffset)
hop = Hopper(N, eng, matlab_hopper)
# hop.mdt_precision = int(ceil(-np.log2(desiredPrecision)))
hop.dtBounds = tuple((1/sqrt(legLength/9.81))*np.array([0.05, 0.2]))
hop.dtNom = 0.04*(1/sqrt(legLength/9.81))
hop.rotationMax = np.pi/8
hop.nOrientationSectors = 1 #int(floor(np.pi/8/desiredPrecision))
print 'hop.nOrientationSectors = %d' % hop.nOrientationSectors
hop.velocityMax = 3.
hop.positionMax = 1.5*rf[0]/legLength
hop.forceMax = 3.
hop.angularVelocityMax = 5.
addThreePlatfomWorld(hop, legLength, 0.3*legLength)
#addFlatWorld(hop, legLength)
hop.constructVisualizer()
m_nlp = hop.constructPyomoModel()
def normL2(m, var):
class Sniffer(object):
"Channel hopping, packet sniffing, parsing and finally storing"
def __init__(self,
db,
interface,
related_interface,
sniffer_name,
enable_hopping,
use_24=True,
use_pop5=False):
self.db = db
self.sniffer_name = sniffer_name
self.interface = interface
self.enable_hopping = enable_hopping
# Check interface existance
if not self._iface_exists(interface):
print "Exiting: Interface %s doesn't exist" % interface
sys.exit(1)
if related_interface and not self._iface_exists(related_interface):
print "Exiting: Related interface %s doesn't exist" % interface
sys.exit(1)
# Logging
header = 'SNIFF'
if sniffer_name:
header += '_' + sniffer_name
self.log = Log(self.db, use_stdout=True, header=header)
# Submodules
self.packet_parser = PacketParser(self.log)
self.hopper = Hopper(self.log, interface, related_interface)
ret = self.hopper.configure(use_24=use_24, use_pop5=use_pop5)
if ret is False:
sys.exit(1)
config.conf.sniff_promisc = 0
self.log.info("Promiscuous mode disabled")
self.watchdog = WatchDog(interval=20)
def _iface_exists(self, iface_name):
"Check if interface exists"
path = '/sys/class/net'
iface_path = os.path.join(path, iface_name)
try:
_ = os.stat(iface_path)
return True
except OSError:
return False
def run(self):
"Sniffer main loop"
begin = time()
pkts_all = 0
sniff_begin = time()
stat_prev = sniff_begin
stat_every = 3 # seconds
while True:
start = time()
# This catches KeyboardInterrupt,
# TODO: Disable this catching + Probably hop on another thread and use prn argument.
# But then - you'd have watchdog problems.
pkts = sendrecv.sniff(iface=self.interface, count=20, timeout=0.1)
pkts_all += len(pkts)
for pkt in pkts:
data = self.packet_parser.parse(pkt)
if data is None:
continue
data['ch'] = self.hopper.channel_number
data['sniffer'] = self.sniffer_name
if ('PROBE_REQ' in data['tags'] or 'PROBE_RESP' in data['tags']
or 'ASSOC_REQ' in data['tags']
or 'DISASS' in data['tags']):
# Increase karma when client traffic is detected
self.hopper.increase_karma()
data['tags'] = list(data['tags'])
self.db.frames.add(data)
now = time()
took = now - start
if stat_prev + stat_every < now:
took = time() - sniff_begin
print "STAT: pkts=%d t_total=%.2fs pps=%.2f swipes=%d avg_swipe_t=%.2f cur_ch=%d" % (
pkts_all,
took,
pkts_all / took,
self.hopper.swipes_total,
took / (self.hopper.swipes_total + 0.001),
self.hopper.channel_number,
)
stat_prev = now
if self.enable_hopping:
ret = self.hopper.karmic_hop()
if ret is False:
break
self.watchdog.dontkillmeplease()
class Sniffer(object):
"Channel hopping, packet sniffing, parsing and finally storing"
def __init__(self, db, interface, related_interface, sniffer_name, enable_hopping,
use_24=True, use_pop5=False):
self.db = db
self.sniffer_name = sniffer_name
self.interface = interface
self.enable_hopping = enable_hopping
# Check interface existance
if not self._iface_exists(interface):
print "Exiting: Interface %s doesn't exist" % interface
sys.exit(1)
if related_interface and not self._iface_exists(related_interface):
print "Exiting: Related interface %s doesn't exist" % interface
sys.exit(1)
# Logging
header = 'SNIFF'
if sniffer_name:
header += '_' + sniffer_name
self.log = Log(self.db, use_stdout=True, header=header)
# Submodules
self.packet_parser = PacketParser(self.log)
self.hopper = Hopper(self.log, interface, related_interface)
ret = self.hopper.configure(use_24=use_24, use_pop5=use_pop5)
if ret is False:
sys.exit(1)
config.conf.sniff_promisc = 0
self.log.info("Promiscuous mode disabled")
self.watchdog = WatchDog(interval=20)
def _iface_exists(self, iface_name):
"Check if interface exists"
path = '/sys/class/net'
iface_path = os.path.join(path, iface_name)
try:
_ = os.stat(iface_path)
return True
except OSError:
return False
def run(self):
"Sniffer main loop"
begin = time()
pkts_all = 0
sniff_begin = time()
stat_prev = sniff_begin
stat_every = 3 # seconds
while True:
start = time()
# This catches KeyboardInterrupt,
# TODO: Disable this catching + Probably hop on another thread and use prn argument.
# But then - you'd have watchdog problems.
pkts = sendrecv.sniff(iface=self.interface, count=20, timeout=0.1)
pkts_all += len(pkts)
for pkt in pkts:
data = self.packet_parser.parse(pkt)
if data is None:
continue
data['ch'] = self.hopper.channel_number
data['sniffer'] = self.sniffer_name
if ('PROBE_REQ' in data['tags'] or
'PROBE_RESP' in data['tags'] or
'ASSOC_REQ' in data['tags'] or
'DISASS' in data['tags']):
# Increase karma when client traffic is detected
self.hopper.increase_karma()
data['tags'] = list(data['tags'])
self.db.frames.add(data)
now = time()
took = now - start
if stat_prev + stat_every < now:
took = time() - sniff_begin
print "STAT: pkts=%d t_total=%.2fs pps=%.2f swipes=%d avg_swipe_t=%.2f cur_ch=%d" % (
pkts_all, took,
pkts_all / took,
self.hopper.swipes_total,
took/(self.hopper.swipes_total + 0.001),
self.hopper.channel_number,
)
stat_prev = now
if self.enable_hopping:
ret = self.hopper.karmic_hop()
if ret is False:
break
self.watchdog.dontkillmeplease()
