class PGPhysicsWorld:
def __init__(self):
# a dynamic world, moving objects or objects which react to other objects should be placed here
self.dynamic_world = BulletWorld()
CollisionGroup.set_collision_rule(self.dynamic_world)
self.dynamic_world.setGravity(Vec3(0, 0, -9.81)) # set gravity
# a static world which used to query position/overlap .etc. Don't implement doPhysics() in this world
self.static_world = BulletWorld()
def report_bodies(self):
dynamic_bodies = \
self.dynamic_world.getNumRigidBodies() + self.dynamic_world.getNumGhosts() + self.dynamic_world.getNumVehicles()
static_bodies = \
self.static_world.getNumRigidBodies() + self.static_world.getNumGhosts() + self.static_world.getNumVehicles()
return "dynamic bodies:{}, static_bodies: {}".format(
dynamic_bodies, static_bodies)
def destroy(self):
self.dynamic_world.clearDebugNode()
self.dynamic_world.clearContactAddedCallback()
self.dynamic_world.clearFilterCallback()
self.static_world.clearDebugNode()
self.static_world.clearContactAddedCallback()
self.static_world.clearFilterCallback()
self.dynamic_world = None
self.static_world = None
def __del__(self):
logging.debug("Physics world is destroyed successfully!")
class EscenaMundo(DirectObject):
"""
* escena del mundo virtual
"""
Nombre = "mundo" # world
def __init__(self, contexto):
DirectObject.__init__(self)
# referencias
self.contexto = contexto
# componentes
self.mundo_fisica = BulletWorld() # physics world
self.debug_fisica = None
self.input = None
self.terreno = None # terrain
self.estaticos = None # static (model instances)
self.animados = list() # animated (model instances, actors)
self.items = None
self.atmosfera = None # atmosphere
self.cntrlcam = None # camera controller
self.texto_info = None # info text
self.debug_hud = None
# parametros
self.cargar_terreno = True
self.cargar_atmosfera = True
def iniciar(self):
log.info("iniciar")
self._establecer_parametros()
#
b = self.contexto.base
# física
self.mundo_fisica.setGravity(Vec3(0, 0, -9.81))
df = BulletDebugNode("debug_fisica")
self.mundo_fisica.setDebugNode(df)
self.debug_fisica = b.render.attachNewNode(df)
self.debug_fisica.hide()
self.accept("f11", self._toggle_debug_fisica)
# input
self.input = Input(self.contexto)
if not self.input.iniciar():
return False
# terreno
if self.cargar_terreno:
self.terreno = Terreno(self.contexto, self.mundo_fisica)
if not self.terreno.iniciar():
return False
# animados (actors)
self._iniciar_animados()
# atmosfera
if self.cargar_atmosfera:
self.atmosfera = Atmosfera(self.contexto)
if not self.atmosfera.iniciar():
return False
# controlador camara
self.cntrlcam = ControladorCamara(self.contexto)
if not self.cntrlcam.iniciar():
return False
# info texto
self.info_texto = OnscreenText(
text="cámara: f1:libre f2:1ºpers f3:3ºpers\n"
"debug física: f11")
# debug hud
self.debug_hud = OnscreenText(parent=b.a2dTopLeft,
text="Debug?",
pos=(0, -0.1),
scale=0.05,
align=TextNode.ALeft,
bg=(1, 1, 1, 0.3))
# pelota (ball)
pelota = b.loader.loadModel("modelos/items/pelota.egg")
pelota.reparentTo(b.render)
pelota.setPos(0, 0.5, 15)
b.messenger.send("establecer_objetivo", [pelota])
# eventos
self.accept("escape-up", self._salir)
# tasks
b.taskMgr.doMethodLater(0.1, self._update_debug_hud,
"Mundo_update_debug_hud")
b.taskMgr.add(self._update, "Mundo_update")
#
return True
def terminar(self):
log.info("terminar")
# tasks
self.contexto.base.taskMgr.remove("Mundo_update_debug_hud")
self.contexto.base.taskMgr.remove("Mundo_update")
# eventos
self.ignoreAll()
# camara
if self.cntrlcam:
self.cntrlcam.terminar()
self.cntrlcam = None
# debug hud
if self.debug_hud:
self.debug_hud.destroy()
self.debug_hud = None
# info texto
if self.info_texto:
self.info_texto.destroy()
self.info_texto = None
# atmosfera
if self.atmosfera:
self.atmosfera.terminar()
self.atmosfera = None
# animados
for animado in self.animados:
animado.terminar()
self.animados = list()
# terreno
if self.terreno:
self.terreno.terminar()
self.terreno = None
# input
if self.input:
self.input.terminar()
self.input = None
# física
self.mundo_fisica.clearDebugNode()
self.debug_fisica.removeNode()
def _establecer_parametros(self):
log.info("_establecer_parametros")
try:
cfg = self.contexto.config["mundo"]
self.cargar_terreno = cfg.getboolean("terreno")
self.cargar_atmosfera = cfg.getboolean("atmosfera")
except ValueError as e:
log.exception("error en el análisis de la configuración: " +
str(e))
def _iniciar_animados(self): # init animated (actors)
log.info("_iniciar_animados")
actor = Persona(self.contexto, self.input)
if actor.iniciar():
self.animados.append(actor)
def _toggle_texto_info(self):
pass
def _toggle_debug_fisica(self):
if self.debug_fisica.isHidden():
self.debug_fisica.show()
else:
self.debug_fisica.hide()
def _salir(self):
self.contexto.base.messenger.send("cambiar_escena", ["inicio"])
def _update_debug_hud(self, task):
info = "Debug\n"
#
info += self.input.obtener_info() + "\n"
if self.atmosfera:
info += self.atmosfera.obtener_info() + "\n"
#
self.debug_hud["text"] = info
return task.again
def _update(self, task):
# física
dt = self.contexto.base.taskMgr.globalClock.getDt()
self.mundo_fisica.doPhysics(dt, 10, 1.0 / 180.0)
#
return task.cont
class World(ShowBase, object):
def __init__(self,
cam_pos=np.array([2.0, 0.5, 2.0]),
lookat_pos=np.array([0, 0, 0.25]),
up=np.array([0, 0, 1]),
fov=40,
w=1920,
h=1080,
lens_type="perspective",
toggle_debug=False,
auto_cam_rotate=False):
"""
:param cam_pos:
:param lookat_pos:
:param fov:
:param w: width of window
:param h: height of window
author: weiwei
date: 2015?, 20201115
"""
# the taskMgr, loader, render2d, etc. are added to builtin after initializing the showbase parental class
super().__init__()
# set up window
winprops = WindowProperties(base.win.getProperties())
winprops.setTitle("WRS Robot Planning and Control System")
base.win.requestProperties(winprops)
self.disableAllAudio()
self.setBackgroundColor(1, 1, 1)
# set up lens
lens = PerspectiveLens()
lens.setFov(fov)
lens.setNearFar(0.001, 5000.0)
if lens_type == "orthographic":
lens = OrthographicLens()
lens.setFilmSize(640, 480)
# disable the default mouse control
self.disableMouse()
self.cam.setPos(cam_pos[0], cam_pos[1], cam_pos[2])
self.cam.lookAt(Point3(lookat_pos[0], lookat_pos[1], lookat_pos[2]),
Vec3(up[0], up[1], up[2]))
self.cam.node().setLens(lens)
# set up slight
## ambient light
ablight = AmbientLight("ambientlight")
ablight.setColor(Vec4(0.2, 0.2, 0.2, 1))
self.ablightnode = self.cam.attachNewNode(ablight)
self.render.setLight(self.ablightnode)
## point light 1
ptlight0 = PointLight("pointlight0")
ptlight0.setColor(Vec4(1, 1, 1, 1))
self._ptlightnode0 = self.cam.attachNewNode(ptlight0)
self._ptlightnode0.setPos(0, 0, 0)
self.render.setLight(self._ptlightnode0)
## point light 2
ptlight1 = PointLight("pointlight1")
ptlight1.setColor(Vec4(.4, .4, .4, 1))
self._ptlightnode1 = self.cam.attachNewNode(ptlight1)
self._ptlightnode1.setPos(self.cam.getPos().length(), 0,
self.cam.getPos().length())
self.render.setLight(self._ptlightnode1)
## point light 3
ptlight2 = PointLight("pointlight2")
ptlight2.setColor(Vec4(.3, .3, .3, 1))
self._ptlightnode2 = self.cam.attachNewNode(ptlight2)
self._ptlightnode2.setPos(-self.cam.getPos().length(), 0,
self.cam.getPos().length())
self.render.setLight(self._ptlightnode2)
# helpers
self.p3dh = p3dh
# self.o3dh = o3dh
self.rbtmath = rm
# set up inputmanager
self.lookatpos = lookat_pos
self.inputmgr = im.InputManager(self, self.lookatpos)
taskMgr.add(self._interaction_update, "interaction", appendTask=True)
# set up rotational cam
if auto_cam_rotate:
taskMgr.doMethodLater(.1, self._rotatecam_update, "rotate cam")
# set window size
props = WindowProperties()
props.setSize(w, h)
self.win.requestProperties(props)
# outline edge shader
# self.set_outlineshader()
# set up cartoon effect
self._separation = 1
self.filter = flt.Filter(self.win, self.cam)
self.filter.setCartoonInk(separation=self._separation)
# self.filter.setViewGlow()
# set up physics world
self.physics_scale = 1e3
self.physicsworld = BulletWorld()
self.physicsworld.setGravity(Vec3(0, 0, -9.81 * self.physics_scale))
taskMgr.add(self._physics_update, "physics", appendTask=True)
globalbprrender = base.render.attachNewNode("globalbpcollider")
debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(False)
debugNode.showNormals(True)
self._debugNP = globalbprrender.attachNewNode(debugNode)
self._debugNP.show()
self.toggledebug = toggle_debug
if toggle_debug:
self.physicsworld.setDebugNode(self._debugNP.node())
self.physicsbodylist = []
# set up render update (TODO, only for dynamics?)
self._internal_update_obj_list = [
] # the nodepath, collision model, or bullet dynamics model to be drawn
self._internal_update_robot_list = []
taskMgr.add(self._internal_update, "internal_update", appendTask=True)
# for remote visualization
self._external_update_objinfo_list = [] # see anime_info.py
self._external_update_robotinfo_list = []
taskMgr.add(self._external_update, "external_update", appendTask=True)
# for stationary models
self._noupdate_model_list = []
def _interaction_update(self, task):
# reset aspect ratio
aspectRatio = self.getAspectRatio()
self.cam.node().getLens().setAspectRatio(aspectRatio)
self.inputmgr.check_mouse1drag()
self.inputmgr.check_mouse2drag()
self.inputmgr.check_mouse3click()
self.inputmgr.check_mousewheel()
self.inputmgr.check_resetcamera()
return task.cont
def _physics_update(self, task):
self.physicsworld.doPhysics(globalClock.getDt(), 20, 1 / 1200)
return task.cont
def _internal_update(self, task):
for robot in self._internal_update_robot_list:
robot.detach() # TODO gen mesh model?
robot.attach_to(self)
for obj in self._internal_update_obj_list:
obj.detach()
obj.attach_to(self)
return task.cont
def _rotatecam_update(self, task):
campos = self.cam.getPos()
camangle = math.atan2(campos[1] - self.lookatpos[1],
campos[0] - self.lookatpos[0])
# print camangle
if camangle < 0:
camangle += math.pi * 2
if camangle >= math.pi * 2:
camangle = 0
else:
camangle += math.pi / 360
camradius = math.sqrt((campos[0] - self.lookatpos[0])**2 +
(campos[1] - self.lookatpos[1])**2)
camx = camradius * math.cos(camangle)
camy = camradius * math.sin(camangle)
self.cam.setPos(self.lookatpos[0] + camx, self.lookatpos[1] + camy,
campos[2])
self.cam.lookAt(self.lookatpos[0], self.lookatpos[1],
self.lookatpos[2])
return task.cont
def _external_update(self, task):
for _external_update_robotinfo in self._external_update_robotinfo_list:
robot_s = _external_update_robotinfo.robot_s
robot_component_name = _external_update_robotinfo.robot_component_name
robot_meshmodel = _external_update_robotinfo.robot_meshmodel
robot_meshmodel_parameter = _external_update_robotinfo.robot_meshmodel_parameters
robot_path = _external_update_robotinfo.robot_path
robot_path_counter = _external_update_robotinfo.robot_path_counter
robot_meshmodel.detach()
robot_s.fk(component_name=robot_component_name,
jnt_values=robot_path[robot_path_counter])
_external_update_robotinfo.robot_meshmodel = robot_s.gen_meshmodel(
tcp_jntid=robot_meshmodel_parameter[0],
tcp_loc_pos=robot_meshmodel_parameter[1],
tcp_loc_rotmat=robot_meshmodel_parameter[2],
toggle_tcpcs=robot_meshmodel_parameter[3],
toggle_jntscs=robot_meshmodel_parameter[4],
rgba=robot_meshmodel_parameter[5],
name=robot_meshmodel_parameter[6])
_external_update_robotinfo.robot_meshmodel.attach_to(self)
_external_update_robotinfo.robot_path_counter += 1
if _external_update_robotinfo.robot_path_counter >= len(
robot_path):
_external_update_robotinfo.robot_path_counter = 0
for _external_update_objinfo in self._external_update_objinfo_list:
obj = _external_update_objinfo.obj
obj_parameters = _external_update_objinfo.obj_parameters
obj_path = _external_update_objinfo.obj_path
obj_path_counter = _external_update_objinfo.obj_path_counter
obj.detach()
obj.set_pos(obj_path[obj_path_counter][0])
obj.set_rotmat(obj_path[obj_path_counter][1])
obj.set_rgba(obj_parameters[0])
obj.attach_to(self)
_external_update_objinfo.obj_path_counter += 1
if _external_update_objinfo.obj_path_counter >= len(obj_path):
_external_update_objinfo.obj_path_counter = 0
return task.cont
def change_debugstatus(self, toggledebug):
if self.toggledebug == toggledebug:
return
elif toggledebug:
self.physicsworld.setDebugNode(self._debugNP.node())
else:
self.physicsworld.clearDebugNode()
self.toggledebug = toggledebug
def attach_internal_update_obj(self, obj):
"""
:param obj: CollisionModel or (Static)GeometricModel
:return:
"""
self._internal_update_obj_list.append(obj)
def detach_internal_update_obj(self, obj):
self._internal_update_obj_list.remove(obj)
obj.detach()
def clear_internal_update_obj(self):
tmp_internal_update_obj_list = self._internal_update_obj_list.copy()
self._internal_update_obj_list = []
for obj in tmp_internal_update_obj_list:
obj.detach()
def attach_internal_update_robot(
self, robot_meshmodel): # TODO robot_meshmodel or robot_s?
self._internal_update_robot_list.append(robot_meshmodel)
def detach_internal_update_robot(self, robot_meshmodel):
tmp_internal_update_robot_list = self._internal_update_robot_list.copy(
)
self._internal_update_robot_list = []
for robot in tmp_internal_update_robot_list:
robot.detach()
def clear_internal_update_robot(self):
for robot in self._internal_update_robot_list:
self.detach_internal_update_robot(robot)
def attach_external_update_obj(self, objinfo):
"""
:param objinfo: anime_info.ObjInfo
:return:
"""
self._external_update_objinfo_list.append(objinfo)
def detach_external_update_obj(self, obj_info):
self._external_update_objinfo_list.remove(obj_info)
obj_info.obj.detach()
def clear_external_update_obj(self):
for obj in self._external_update_objinfo_list:
self.detach_external_update_obj(obj)
def attach_external_update_robot(self, robotinfo):
"""
:param robotinfo: anime_info.RobotInfo
:return:
"""
self._external_update_robotinfo_list.append(robotinfo)
def detach_external_update_robot(self, robot_info):
self._external_update_robotinfo_list.remove(robot_info)
robot_info.robot_meshmodel.detach()
def clear_external_update_robot(self):
for robot in self._external_update_robotinfo_list:
self.detach_external_update_robot(robot)
def attach_noupdate_model(self, model):
model.attach_to(self)
self._noupdate_model_list.append(model)
def detach_noupdate_model(self, model):
model.detach()
self._noupdate_model_list.remove(model)
def clear_noupdate_model(self):
for model in self._noupdate_model_list:
model.detach()
self._noupdate_model_list = []
def change_campos(self, campos):
self.cam.setPos(campos[0], campos[1], campos[2])
self.inputmgr = im.InputManager(self, self.lookatpos)
def change_lookatpos(self, lookatpos):
"""
This function is questionable
as lookat changes the rotation of the camera
:param lookatpos:
:return:
author: weiwei
date: 20180606
"""
self.cam.lookAt(lookatpos[0], lookatpos[1], lookatpos[2])
self.lookatpos = lookatpos
self.inputmgr = im.InputManager(self, self.lookatpos)
def change_campos_and_lookatpos(self, campos, lookatpos):
self.cam.setPos(campos[0], campos[1], campos[2])
self.cam.lookAt(lookatpos[0], lookatpos[1], lookatpos[2])
self.lookatpos = lookatpos
self.inputmgr = im.InputManager(self, self.lookatpos)
def set_cartoonshader(self, switchtoon=False):
"""
set cartoon shader, the following program is a reference
https://github.com/panda3d/panda3d/blob/master/samples/cartoon-shader/advanced.py
:return:
author: weiwei
date: 20180601
"""
this_dir, this_filename = os.path.split(__file__)
if switchtoon:
lightinggen = Filename.fromOsSpecific(
os.path.join(this_dir, "shaders", "lighting_gen.sha"))
tempnode = NodePath("temp")
tempnode.setShader(loader.loadShader(lightinggen))
self.cam.node().setInitialState(tempnode.getState())
# self.render.setShaderInput("light", self.cam)
self.render.setShaderInput("light", self._ablightnode)
normalsBuffer = self.win.makeTextureBuffer("normalsBuffer", 0, 0)
normalsBuffer.setClearColor(Vec4(0.5, 0.5, 0.5, 1))
normalsCamera = self.makeCamera(normalsBuffer,
lens=self.cam.node().getLens(),
scene=self.render)
normalsCamera.reparentTo(self.cam)
normalgen = Filename.fromOsSpecific(
os.path.join(this_dir, "shaders", "normal_gen.sha"))
tempnode = NodePath("temp")
tempnode.setShader(loader.loadShader(normalgen))
normalsCamera.node().setInitialState(tempnode.getState())
drawnScene = normalsBuffer.getTextureCard()
drawnScene.setTransparency(1)
drawnScene.setColor(1, 1, 1, 0)
drawnScene.reparentTo(render2d)
self.drawnScene = drawnScene
self.separation = 0.001
self.cutoff = 0.05
inkGen = Filename.fromOsSpecific(
os.path.join(this_dir, "shaders", "ink_gen.sha"))
drawnScene.setShader(loader.loadShader(inkGen))
drawnScene.setShaderInput("separation", Vec4(0, 0, self.separation, 0))
drawnScene.setShaderInput("cutoff", Vec4(self.cutoff))
def set_outlineshader(self):
"""
document 1: https://qiita.com/nmxi/items/bfd10a3b3f519878e74e
document 2: https://docs.panda3d.org/1.10/python/programming/shaders/list-of-cg-inputs
:return:
author: weiwei
date: 20180601, 20201210osaka
"""
depth_sha = """
void vshader(float4 vtx_position : POSITION,
float4 vtx_normal : NORMAL,
uniform float4x4 mat_modelproj,
uniform float4x4 mat_modelview,
out float4 l_position : POSITION,
out float4 l_color0: COLOR0) {
l_position = mul(mat_modelproj, vtx_position);
float depth = l_position.a*.1;
//l_color0 = vtx_position + float4(depth, depth, depth, 1);
l_color0 = float4(depth, depth, depth, 1);
}
void fshader(float4 l_color0: COLOR0,
uniform sampler2D tex_0 : TEXUNIT0,
out float4 o_color : COLOR) {
o_color = l_color0;
}"""
outline_sha = """
void vshader(float4 vtx_position : POSITION,
float2 vtx_texcoord0 : TEXCOORD0,
uniform float4x4 mat_modelproj,
out float4 l_position : POSITION,
out float2 l_texcoord0 : TEXCOORD0)
{
l_position = mul(mat_modelproj, vtx_position);
l_texcoord0 = vtx_texcoord0;
}
void fshader(float2 l_texcoord0 : TEXCOORD0,
uniform sampler2D tex_0 : TEXUNIT0,
uniform float2 sys_windowsize,
out float4 o_color : COLOR)
{
float sepx = 1/sys_windowsize.x;
float sepy = 1/sys_windowsize.y;
float4 color0 = tex2D(tex_0, l_texcoord0);
float2 texcoord1 = l_texcoord0+float2(sepx, 0);
float4 color1 = tex2D(tex_0, texcoord1);
float2 texcoord2 = l_texcoord0+float2(0, sepy);
float4 color2 = tex2D(tex_0, texcoord2);
float2 texcoord3 = l_texcoord0+float2(-sepx, 0);
float4 color3 = tex2D(tex_0, texcoord3);
float2 texcoord4 = l_texcoord0+float2(0, -sepy);
float4 color4 = tex2D(tex_0, texcoord4);
float2 texcoord5 = l_texcoord0+float2(sepx, sepy);
float4 color5 = tex2D(tex_0, texcoord5);
float2 texcoord6 = l_texcoord0+float2(-sepx, -sepy);
float4 color6 = tex2D(tex_0, texcoord6);
float2 texcoord7 = l_texcoord0+float2(-sepx, sepy);
float4 color7 = tex2D(tex_0, texcoord7);
float2 texcoord8 = l_texcoord0+float2(sepx, -sepy);
float4 color8 = tex2D(tex_0, texcoord8);
float2 texcoord9 = l_texcoord0+float2(2*sepx, 0);
float4 color9 = tex2D(tex_0, texcoord9);
float2 texcoord10 = l_texcoord0+float2(-2*sepx, 0);
float4 color10 = tex2D(tex_0, texcoord10);
float2 texcoord11 = l_texcoord0+float2(0, 2*sepy);
float4 color11 = tex2D(tex_0, texcoord11);
float2 texcoord12 = l_texcoord0+float2(0, -2*sepy);
float4 color12 = tex2D(tex_0, texcoord12);
o_color = (color0-color1).x > .005 || (color0-color2).x > .005 || (color0-color3).x > .005 ||
(color0-color4).x > .005 || (color0-color5).x > .005 || (color0-color6).x > .005 ||
(color0-color7).x > .005 || (color0-color8).x > .005 || (color0-color9).x > .005 ||
(color0-color10).x > .005 || (color0-color11).x > .005 || (color0-color12).x > .005 ?
float4(0, 0, 0, 1) : float4(0, 0, 0, 0);
}"""
depthBuffer = self.win.makeTextureBuffer("depthBuffer", 0, 0)
depthBuffer.setClearColor(Vec4(1, 1, 1, 1))
depthCamera = self.makeCamera(depthBuffer,
lens=self.cam.node().getLens(),
scene=self.render)
depthCamera.reparentTo(self.cam)
tempnode = NodePath("depth")
tempnode.setShader(Shader.make(depth_sha, Shader.SL_Cg))
depthCamera.node().setInitialState(tempnode.getState())
drawnScene = depthBuffer.getTextureCard()
drawnScene.reparentTo(render2d)
drawnScene.setTransparency(1)
drawnScene.setColor(1, 1, 1, 0)
drawnScene.setShader(Shader.make(outline_sha, Shader.SL_Cg))
