def world_setup(env, render, mydir):
env.disableMouse()
# Load the environment model.
env.scene = env.loader.loadModel(mydir + "/models/city.egg")
env.scene.reparentTo(env.render)
env.scene.setScale(1, 1, 1)
env.scene.setPos(0, 0, 0)
# Load the skybox
# env.skybox = env.loader.loadModel(mydir + "/models/skybox.egg")
# env.skybox.setScale(100,100,100)
# env.skybox.setPos(0,0,-500)
# env.skybox.reparentTo(env.render)
# Also add an ambient light and set sky color.
skycol = panda3d.core.VBase3(135 / 255.0, 206 / 255.0, 235 / 255.0)
env.set_background_color(skycol)
alight = AmbientLight("sky")
alight.set_color(panda3d.core.VBase4(skycol * 0.04, 1))
alight_path = render.attachNewNode(alight)
render.set_light(alight_path)
# 4 perpendicular lights (flood light)
dlight1 = DirectionalLight('directionalLight')
dlight1.setColor(panda3d.core.Vec4(0.3,0.3,0.3,0.3))
dlight1NP = render.attachNewNode(dlight1)
dlight1NP.setHpr(0,0,0)
dlight2 = DirectionalLight('directionalLight')
dlight2.setColor(panda3d.core.Vec4(0.3,0.3,0.3,0.3))
dlight2NP = render.attachNewNode(dlight2)
dlight2NP.setHpr(-90,0,0)
dlight3 = DirectionalLight('directionalLight')
dlight3.setColor(panda3d.core.Vec4(0.3,0.3,0.3,0.3))
dlight3NP = render.attachNewNode(dlight3)
dlight3NP.setHpr(-180,0,0)
dlight4 = DirectionalLight('directionalLight')
dlight4.setColor(panda3d.core.Vec4(0.3,0.3,0.3,0.3))
dlight4NP = render.attachNewNode(dlight4)
dlight4NP.setHpr(-270,0,0)
render.setLight(dlight1NP)
render.setLight(dlight2NP)
render.setLight(dlight3NP)
render.setLight(dlight4NP)
# 1 directional light (Sun)
dlight = DirectionalLight('directionalLight')
dlight.setColor(panda3d.core.Vec4(1, 1, 1, 1)) # directional light is dim green
dlight.getLens().setFilmSize(panda3d.core.Vec2(50, 50))
dlight.getLens().setNearFar(-100, 100)
dlight.setShadowCaster(True, 1028*2, 1028*2)
# dlight.show_frustum()
dlightNP = render.attachNewNode(dlight)
dlightNP.setHpr(0,-65,0)
#Turning shader and lights on
render.setShaderAuto()
render.setLight(dlightNP)
def createLighting(self):
light = DirectionalLight("light")
light.setColor(VBase4(0.4, 0.4, 0.4, 1))
light.setShadowCaster(True)
light.getLens().setNearFar(100.0, 400.0)
light.getLens().setFilmSize(400, 400)
# light.showFrustum()
np = self.render.attachNewNode(light)
np.setPos(100, -100, 200)
np.lookAt(0, 0, -100)
self.render.setLight(np)
light = AmbientLight("ambient")
light.setColor(VBase4(0.2, 0.2, 0.2, 1))
np = self.render.attachNewNode(light)
self.render.setLight(np)
def createLights(self):
"""Create an ambient light and a spotlight for shadows"""
self.render.clearLight()
alight = AmbientLight("ambientLight")
alight.setColor(Vec4(0.7, 0.7, 0.7, 1))
alightNP = self.worldRender.attachNewNode(alight)
self.worldRender.setLight(alightNP)
# Create a directional light for shadows
dlight = DirectionalLight("dLight")
dlight.setColor(Vec4(0.6, 0.6, 0.6, 1))
dlight.setShadowCaster(True, 1024, 1024)
dlight.getLens().setNearFar(1, 15)
dlight.getLens().setFilmSize(128, 128)
dlightNP = self.worldRender.attachNewNode(dlight)
dlightNP.setPos(0, 0, 10)
dlightNP.lookAt(0, 0, 0)
self.worldRender.setLight(dlightNP)
def setUpLights(self):
# ambient
#alight=AmbientLight("alight")
#alight.setColor(Vec4(Terrain.COLOR_AMB_50))
#alightP=self.base.render.attachNewNode(alight)
# point
plight = PointLight("plight")
plight.setColor(Vec4(Terrain.COLOR_WHITE))
self.plightP = self.base.render.attachNewNode(plight)
self.plightP.setPos(14, -30, 17)
# directional
# sun
sun = DirectionalLight("sun")
sun.setColor(Vec4(Terrain.COLOR_WHITE))
#sun.setShadowCaster(True, 1024, 1024)
sun.getLens().setFilmSize(Vec2(100, 100))
sun.getLens().setNearFar(10, 200)
sun.getLens().setFov(200)
#sun.showFrustum()
sunP = self.base.render.attachNewNode(sun)
sunP.setPos(0, -2, 20)
sunP.setHpr(-60, -90, -30)
# sky
sunSky = DirectionalLight("sunSky")
sunSky.setColor(Vec4(Terrain.COLOR_WHITE))
sunSkyP = self.base.render.attachNewNode(sunSky)
sunSkyP.setPos(-14, 30, -17)
sunSkyP.setHpr(-10, 60, -10)
#
#self.base.render.setLight(alightP)
self.base.render.setLight(sunP)
self.base.render.setLight(self.plightP)
def setupLights(self):
ambientLight = AmbientLight("ambientLight")
ambientLight.setColor((0.2, 0.2, 0.2, 1.0))
directionalLight = DirectionalLight("directionalLight")
directionalLight.setDirection(Vec3(0.0, 0.2, -1.0).normalized())
directionalLight.setColor((0.9, 0.9, 0.9, 1.0))
directionalLight.setShadowCaster(True, 1024, 1024)
directionalLight.getLens().setNearFar(-1000.0, 1000)
directionalLight.getLens().setFilmSize(200, 200)
directionalLight.getLens().setViewVector(
directionalLight.getDirection(), Vec3(1, 0, 0))
self.directionalLightNode = render.attachNewNode(directionalLight)
render.setLight(self.directionalLightNode)
render.setLight(render.attachNewNode(ambientLight))
def __init__(self, base):
self.base = base
# model
self.model = self.base.loader.loadModel("models/terrain")
self.model.reparentTo(self.base.render)
# lights
# ambient
alight = AmbientLight("alight")
alight.setColor(Vec4(0.4, 0.4, 0.4, 1))
alightP = self.base.render.attachNewNode(alight)
self.base.render.setLight(alightP)
# directional
sun = DirectionalLight("sun")
sun.setColor(Vec4(1, 0.973, 0.491, 1))
sun.setShadowCaster(True, 4096, 4096)
sun.getLens().setFilmSize(Vec2(30, 30))
sun.getLens().setNearFar(10, 100)
sun.getLens().setFov(100)
sunP = self.base.render.attachNewNode(sun)
sunP.setHpr(10, -60, 10)
self.base.render.setLight(sunP)
def __init__(self):
ShowBase.__init__(self)
self.calibrated = False
self.disableMouse()
self.time_total_img = []
self.time_total_sens = []
# ENV SETUP
self.env = quad(time_int_step, max_timesteps, direct_control=1, T=T)
self.sensor = sensor(self.env)
self.aux_dl = dl_in_gen(T, self.env.state_size, self.env.action_size)
self.error = []
state_dim = self.aux_dl.deep_learning_in_size
self.policy = ActorCritic(state_dim, action_dim=4, action_std=0).to(device)
#CONTROLLER SETUP
try:
self.policy.load_state_dict(torch.load('./controller/PPO_continuous_solved_drone.pth',map_location=device))
print('Saved policy loaded')
except:
print('Could not load policy')
sys.exit(1)
n_parameters = sum(p.numel() for p in self.policy.parameters())
print('Neural Network Number of Parameters: %i' %n_parameters)
# Load the environment model.
self.scene = self.loader.loadModel(mydir + "/models/city.egg")
self.scene.reparentTo(self.render)
self.scene.setScale(1, 1, 1)
self.scene.setPos(0, 0, 0)
# Load the skybox
self.skybox = self.loader.loadModel(mydir + "/models/skybox.egg")
self.skybox.setScale(100,100,100)
self.skybox.setPos(0,0,-500)
self.skybox.reparentTo(self.render)
# Also add an ambient light and set sky color.
skycol = panda3d.core.VBase3(135 / 255.0, 206 / 255.0, 235 / 255.0)
self.set_background_color(skycol)
alight = AmbientLight("sky")
alight.set_color(panda3d.core.VBase4(skycol * 0.04, 1))
alight_path = render.attachNewNode(alight)
render.set_light(alight_path)
# 4 perpendicular lights (flood light)
dlight1 = DirectionalLight('directionalLight')
dlight1.setColor(panda3d.core.Vec4(0.3,0.3,0.3,0.3))
dlight1NP = render.attachNewNode(dlight1)
dlight1NP.setHpr(0,0,0)
dlight2 = DirectionalLight('directionalLight')
dlight2.setColor(panda3d.core.Vec4(0.3,0.3,0.3,0.3))
dlight2NP = render.attachNewNode(dlight2)
dlight2NP.setHpr(-90,0,0)
dlight3 = DirectionalLight('directionalLight')
dlight3.setColor(panda3d.core.Vec4(0.3,0.3,0.3,0.3))
dlight3NP = render.attachNewNode(dlight3)
dlight3NP.setHpr(-180,0,0)
dlight4 = DirectionalLight('directionalLight')
dlight4.setColor(panda3d.core.Vec4(0.3,0.3,0.3,0.3))
dlight4NP = render.attachNewNode(dlight4)
dlight4NP.setHpr(-270,0,0)
render.setLight(dlight1NP)
render.setLight(dlight2NP)
render.setLight(dlight3NP)
render.setLight(dlight4NP)
# 1 directional light (Sun)
dlight = DirectionalLight('directionalLight')
dlight.setColor(panda3d.core.Vec4(1, 1, 1, 1)) # directional light is dim green
dlight.getLens().setFilmSize(panda3d.core.Vec2(50, 50))
dlight.getLens().setNearFar(-100, 100)
dlight.setShadowCaster(True, 4096*2, 4096*2)
# dlight.show_frustum()
dlightNP = render.attachNewNode(dlight)
dlightNP.setHpr(0,-65,0)
#Turning shader and lights on
render.setShaderAuto()
render.setLight(dlightNP)
# Add the spinCameraTask procedure to the task manager.
self.taskMgr.add(self.calibrate, 'Camera Calibration')
self.taskMgr.add(self.drone_position_task, 'Drone Position')
# Load and transform the quadrotor actor.
self.quad = self.loader.loadModel(mydir + '/models/quad.egg')
self.quad.reparentTo(self.render)
self.prop_1 = self.loader.loadModel(mydir + '/models/prop.egg')
self.prop_1.setPos(-0.26,0,0)
self.prop_1.reparentTo(self.quad)
self.prop_2 = self.loader.loadModel(mydir + '/models/prop.egg')
self.prop_2.setPos(0,0.26,0)
self.prop_2.reparentTo(self.quad)
self.prop_3 = self.loader.loadModel(mydir + '/models/prop.egg')
self.prop_3.setPos(0.26,0,0)
self.prop_3.reparentTo(self.quad)
self.prop_4 = self.loader.loadModel(mydir + '/models/prop.egg')
self.prop_4.setPos(0,-0.26,0)
self.prop_4.reparentTo(self.quad)
# Load the checkerboard actor
self.checker = self.loader.loadModel(mydir+ '/models/checkerboard.egg')
self.checker.reparentTo(self.render)
self.checker_scale = 0.5
self.checker_sqr_size = 0.2046
self.checker.setScale(self.checker_scale, self.checker_scale, 1)
self.checker.setPos(3*self.checker_scale*self.checker_sqr_size, 2.5*self.checker_scale*self.checker_sqr_size, 0.001)
#env cam
self.cam.node().getLens().setFilmSize(36, 24)
self.cam.node().getLens().setFocalLength(45)
self.cam.setPos(5,5,7)
self.cam.reparentTo(self.render)
self.cam.lookAt(self.quad)
if IMG_POS_DETER:
self.taskMgr.add(self.pos_deter, 'Position Determination')
window_size = (self.win.getXSize(), self.win.getYSize())
self.buffer = self.win.makeTextureBuffer('Buffer', *window_size, None, True)
self.tex = self.buffer.getTexture()
self.cam_1 = self.makeCamera(self.buffer)
self.cam_1.setName('cam_1')
self.cam_1.node().getLens().setFilmSize(36, 24)
self.cam_1.node().getLens().setFocalLength(45)
self.cam_1.reparentTo(self.quad)
self.cam_1.setPos(0,0,0.01)
self.cam_1.lookAt(self.quad)
self.pipe = panda3d.core.GraphicsPipeSelection.get_global_ptr().make_module_pipe('pandagl')
input('Start?')
class NewSpaceCraft:
def __init__(self, base, quality=0, sun_angle=0):
self.base = base
folder = base.params("space_craft_folder")
self.model = base.loader.load_model(folder + "core.egg")
self.wheel = base.loader.load_model(folder + "wheel.egg")
self.sp = base.loader.load_model(folder + "solar_panel.egg")
self.model.reparentTo(base.render)
self.wheel.reparentTo(self.model)
self.rest_pos = NodePath("rest_pos")
self.rest_pos.reparent_to(self.model)
self.rest_pos.set_pos(LVector3f(0.0, -0.75, -0.24))
# self.wheel_lights = base.loader.load_model(folder + "wheel_lights.egg")
# self.wheel_lights.set_color(1, 1, 0)
# self.wheel_lights.reparentTo(self.wheel)
# self.wheel_lights.setLightOff()
# instanciating
self.placeholders = []
z1 = 0.4215
z2 = 0.67935
x1 = 1.136
x2 = 2.6915
for i, pos in enumerate(
((x1, 0, z1), (x2, 0, z1), (-x1, 0, z1), (-x2, 0, z1), (x1, 0, z2),
(x2, 0, z2), (-x1, 0, z2), (-x2, 0, z2))):
self.placeholders.append(self.model.attachNewNode("p" + str(i)))
self.placeholders[-1].set_pos(pos)
self.sp.instanceTo(self.placeholders[-1])
# wheel spinning task
self.spin_velocity = 0.01
if self.spin_velocity > 0.:
self.spin_task = self.wheel.hprInterval(duration=1 /
self.spin_velocity,
hpr=(360, 0, 0))
self.spin_task.loop()
# model light
self.sun_light = DirectionalLight('SUN_LIGHT_ON_SPACE_CRAFT')
self.sun_light.setColor((1., 1., 1., 1))
self.sun_light_node = self.base.render.attachNewNode(self.sun_light)
# self.sun_light_node.setHpr(-135, 0, 0)
self.base.render.setLight(self.sun_light_node)
if quality > 0:
print(quality * 512)
self.sun_light.setShadowCaster(True, quality * 512, quality * 512)
# self.sun_light.getLens().setFilmSize(60., 40.)
self.sun_light.getLens().setFilmSize(6., 4.)
self.sun_light.getLens().setNearFar(2., 6.)
self.sun_light_node.lookAt(NodePath(self.base.sun))
self.sun_light_node.set_pos(0, 3.5, 1)
self.sun_light_node.set_h(self.sun_light_node.get_h() + 180)
# self.model.set_r(180)
self.model.set_p(-90 + sun_angle)
# self.model.set_h(180)
self.model.set_bin('fixed', 10)
self.set_solar_panel_angle(70)
# self.show_shuttle_rest_pos()
# self.model.set_depth_write(0)
# self.model.set_depth_test(0)
def set_hpr(self, h, p, r):
self.model.set_hpr(h, p, r)
def get_shuttle_rest_pos(self):
return self.rest_pos.get_pos()
def get_shuttle_rest_hpr(self):
return self.rest_pos.get_hpr()
def get_connection_pos_and_hpr(self):
node = NodePath('test')
node.set_pos(0, 0, 0)
node.set_hpr(0, 90, 0)
node.reparent_to(self.rest_pos)
node.wrtReparentTo(self.base.render)
return node.get_pos(), node.get_hpr()
def connect_to_shuttle(self, node):
node.set_pos(0, 0, 0)
node.set_hpr(0, 90, 0)
node.reparent_to(self.rest_pos)
node.wrtReparentTo(self.base.render)
def show_shuttle_rest_pos(self):
arrow = Arrow(self.base)
arrow.set_color(0.5, 0.5, 0)
arrow.reparent_to(self.rest_pos)
arrow.look_at(LVector3f(0, 0, 1))
# l = Lamp(self.base, 'lamp', self.get_shuttle_rest_pos(), show_model=True)
@staticmethod
def get_shuttle_rest_direction():
return LVector3f(1, 0, 0)
def show_bounds(self):
self.sun_light.showFrustum()
for child in self.sun_light.getChildren():
if child.name == "frustum":
NodePath(child).setLightOff()
NodePath(child).set_color(1, 1, 0, 1)
NodePath(child).set_bin('fixed', 10)
self.model.showTightBounds()
def set_solar_panel_angle(self, angle):
self.sp.set_p(angle)
def solar_panel_increment(self, angle):
self.sp.set_p(self.sp.get_p() + angle)
print("sp :", self.sp.get_p())
def spin(self, angle, value):
if angle == 'p':
self.model.set_p(self.model.get_p() + value)
elif angle == 'h':
self.model.set_h(self.model.get_h() + value)
elif angle == 'r':
self.model.set_r(self.model.get_r() + value)
print(self.model.get_hpr())
def move_solar_panels(self, new_angle=None):
if new_angle is None:
new_angle = 180 * random()
if 0 <= new_angle <= 180:
angle = self.sp.get_h()
time = (new_angle - angle) * 0.1
if time < 0:
time = -time
task = self.sp.hprInterval(duration=time, hpr=(0, new_angle, 0))
task.start()
def get_light(self):
return self.sun_light_node
def __init__(self):
ShowBase.__init__(self)
self.scene = self.loader.loadModel(models_dir + "hallway.bam") # Load the environment model
self.scene.reparentTo(self.render)
self.scene.setScale(1, 1, 1)
self.scene.setPos(0, 0, 1)
self.scene.setHpr(90, 0, 0)
# Add an ambient light and set sky color
sky_col = VBase3(135 / 255.0, 206 / 255.0, 235 / 255.0)
self.set_background_color(sky_col)
alight = AmbientLight("sky")
alight.set_color(VBase4(sky_col * 0.04, 1))
alight_path = self.render.attachNewNode(alight)
self.render.set_light(alight_path)
# # 4 perpendicular lights (flood light)
for light_no in range(4):
d_light = DirectionalLight('directionalLight')
d_light.setColor(Vec4(*([0.3] * 4)))
d_light_NP = self.render.attachNewNode(d_light)
d_light_NP.setHpr(-90 * light_no, 0, 0)
self.render.setLight(d_light_NP)
# # 1 directional light (Sun)
sun_light = DirectionalLight('directionalLight')
sun_light.setColor(Vec4(*([0.7] * 4))) # directional light is dim green
sun_light.getLens().setFilmSize(Vec2(0.8, 0.8))
sun_light.getLens().setNearFar(-0.3, 12)
sun_light.setShadowCaster(True, 2 ** 7, 2 ** 7)
self.dlightNP = self.render.attachNewNode(sun_light)
self.dlightNP.setHpr(0, -65, 0)
# Turning shader and lights on
self.render.setLight(self.dlightNP)
# Load and transform the quadrotor actor.
self.quad_model = self.loader.loadModel(models_dir + f'{quad_model_filename}.egg')
self.prop_models = []
for prop_no in range(4):
prop = self.loader.loadModel(models_dir + 'propeller.egg')
x = 0 if prop_no % 2 == 1 else (-0.26 if prop_no == 0 else 0.26)
y = 0 if prop_no % 2 == 0 else (-0.26 if prop_no == 3 else 0.26)
prop.setPos(x, y, 0)
prop.reparentTo(self.quad_model)
self.prop_models.append(prop)
self.prop_models = tuple(self.prop_models)
# self.quad_model.reparentTo(self.scene)
self.quad_model.setPos(0, 0, 2)
self.quad_neutral_hpr = (90, 0, 0)
self.quad_model.setHpr(*self.quad_neutral_hpr)
# env cam
self.cam_neutral_pos = (0, -4, 3)
self.cam.reparentTo(self.scene)
# self.cam_neutral_pos = (-4, 0, 1)
# self.cam.reparentTo(self.quad_model)
self.cam.setPos(*self.cam_neutral_pos)
self.cam.lookAt(self.quad_model)
self.enableParticles()
node = NodePath("PhysicsNode")
node.reparentTo(self.scene)
self.actor_node = ActorNode("quadrotor-physics")
# self.actor_node.getPhysicsObject().setMass(1)
self.actor_node_physics = node.attachNewNode(self.actor_node)
self.physicsMgr.attachPhysicalNode(self.actor_node)
self.quad_model.reparentTo(self.actor_node_physics)
# add gravity
# gravity_force_node = ForceNode('world-forces')
# gravityForce = LinearVectorForce(0, 0, -0.1) # gravity acceleration
# gravity_force_node.addForce(gravityForce)
# self.physicsMgr.addLinearForce(gravityForce)
self.time = datetime.datetime.today().strftime('%Y-%m-%d-%H.%M.%S')
self.simulation_folder = "\sims\\" + self.time + '\\'
self.simulation_folder_path = ROOT_DIR + self.simulation_folder
os.makedirs(self.simulation_folder_path)
self.movements = ''
self.taskMgr.add(self.camera_move, 'Camera Movement')
self.taskMgr.add(self.quad_move, 'Quad Movement')
self.taskMgr.add(self.rotate_propellers, 'Propellers Rotation')
self.taskMgr.add(self.save_image, 'Screenshot Capture')
# self.buffer: GraphicsBuffer = self.win.makeTextureBuffer(name='buffer', x_size=84, y_size=84, tex=None, to_ram=True)
# self.buffer.setActive(1)
self.images = []
self.image_index = 1
