import math
from lasagne.updates import get_or_compute_grads
import theano
import theano.tensor as T
from TheanoPhysicsSystem import TheanoRigid3DBodyEngine, theano_to_print
import lasagne
import sys
import numpy as np
sys.setrecursionlimit(10**6)
# step 1: load the physics model
engine = TheanoRigid3DBodyEngine()
engine.load_robot_model("robotmodel/full_predator.json")
spine_id = engine.get_object_index("spine")
engine.compile()
# step 2: build the model, controller and engine for simulation
DT = 0.001
total_time = 0.002
def build_objectives(states_list):
positions, velocities, rotations = states_list
#theano_to_print.extend([rotations[-1,6,:,:]])
return (rotations[-1, spine_id, :, :] -
engine.getInitialState()[2][spine_id, :, :]).norm(L=1, axis=(0, 1))
def build_model():
def __init__(self):
ShowBase.__init__(self)
self.t = 0
self.starttime = None
self.setFrameRateMeter(True)
cour = self.loader.loadFont('cmtt12.egg')
self.textObject = OnscreenText(font=cour,
text='abcdefghijklmnopqrstuvwxyz',
pos=(0, -0.045),
parent=self.a2dTopCenter,
bg=(0, 0, 0, 0.5),
fg=(1, 1, 1, 1),
scale=0.07,
mayChange=True)
cm = CardMaker("ground")
cm.setFrame(-2000, 2000, -2000, 2000)
cm.setUvRange(Point2(-2000 / 5, -2000 / 5), Point2(2000 / 5, 2000 / 5))
tmp = self.render.attachNewNode(cm.generate())
tmp.reparentTo(self.render)
self.camLens.setNear(0.1)
tmp.setPos(0, 0, 0)
tmp.lookAt((0, 0, -2))
tmp.setColor(1.0, 1.0, 1.0, 0.)
#tmp.setTexScale(TextureStage.getDefault(), 1, 1)
tex = self.loader.loadTexture('textures/grid2.png')
tex.setWrapU(Texture.WMRepeat)
tex.setWrapV(Texture.WMRepeat)
tmp.setTexture(tex, 1)
self.setBackgroundColor(0.0, 191.0 / 255.0, 1.0,
1.0) #color of the sky
ambientLight = AmbientLight('ambientLight')
ambientLight.setColor(Vec4(0.2, 0.2, 0.2, 1))
ambientLightNP = self.render.attachNewNode(ambientLight)
self.render.setLight(ambientLightNP)
# Directional light 01
directionalLight = DirectionalLight('directionalLight')
directionalLight.setColor(Vec4(0.8, 0.8, 0.8, 1))
directionalLightNP = self.render.attachNewNode(directionalLight)
# This light is facing backwards, towards the camera.
directionalLightNP.setHpr(-120, -50, 0)
directionalLightNP.node().setScene(self.render)
directionalLightNP.node().setShadowCaster(True)
directionalLightNP.node().getLens().setFov(40)
directionalLightNP.node().getLens().setNearFar(10, 100)
self.render.setLight(directionalLightNP)
# Add the spinCameraTask procedure to the task manager.
self.taskMgr.add(self.spinCameraTask, "SpinCameraTask")
self.physics = TheanoRigid3DBodyEngine()
self.benchmark_physics = None #Rigid3DBodyEngine()
# Load the environment model.
self.objects = dict()
#self.load_robot_model("robotmodel/test.json")
self.load_robot_model("robotmodel/predator.json")
#self.load_robot_model("robotmodel/simple_predator.json")
if self.benchmark_physics:
self.benchmark_physics.compile()
print "Compiling..."
self.positions, self.velocities, self.rotations = self.physics.getInitialState(
)
self.physics.compile()
import theano.tensor as T
positions = T.fmatrix()
velocities = T.fmatrix()
rotations = T.ftensor3()
a, b, c = self.physics.step_from_this_state(
(positions, velocities, rotations),
dt=0.001,
motor_signals=[-1, 1, -1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
self.timestep = theano.function(
inputs=[positions, velocities, rotations],
outputs=[a, b, c],
allow_input_downcast=True)