def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.debugNP.node().showWireframe(True)
self.debugNP.node().showConstraints(True)
self.debugNP.node().showBoundingBoxes(False)
self.debugNP.node().showNormals(False)
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Box A
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
bodyA = BulletRigidBodyNode('Box A')
bodyNP = self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(-3, 0, 4)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box B
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, 0)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Slider
frameA = TransformState.makePosHpr(Point3(2, 0, 0), Vec3(0, 0, 45))
frameB = TransformState.makePosHpr(Point3(0, -3, 0), Vec3(0, 0, 0))
slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True)
slider.setDebugDrawSize(2.0)
slider.setLowerLinearLimit(0)
slider.setUpperLinearLimit(6)
slider.setLowerAngularLimit(-60)
slider.setUpperAngularLimit(60)
self.world.attachConstraint(slider)
def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.debugNP.node().showWireframe(True)
self.debugNP.node().showConstraints(True)
self.debugNP.node().showBoundingBoxes(False)
self.debugNP.node().showNormals(False)
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Box A
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
bodyA = BulletRigidBodyNode('Box A')
bodyNP = self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(-3, 0, 4)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box B
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, 0)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Slider
frameA = TransformState.makePosHpr(Point3(2, 0, 0), Vec3(0, 0, 45))
frameB = TransformState.makePosHpr(Point3(0, -3, 0), Vec3(0, 0, 0))
slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True)
slider.setDebugDrawSize(2.0)
slider.setLowerLinearLimit(0)
slider.setUpperLinearLimit(6)
slider.setLowerAngularLimit(-60)
slider.setUpperAngularLimit(60)
self.world.attachConstraint(slider)
def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.debugNP.node().showWireframe(True)
self.debugNP.node().showConstraints(True)
self.debugNP.node().showBoundingBoxes(False)
self.debugNP.node().showNormals(False)
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Box A
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
bodyA = BulletRigidBodyNode('Box A')
bodyNP = self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(-2, 0, 4)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box B
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, 0)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Cone
frameA = TransformState.makePosHpr(Point3(0, 0, -2), Vec3(0, 0, 90))
frameB = TransformState.makePosHpr(Point3(-5, 0, 0), Vec3(0, 0, 0))
cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB)
cone.setDebugDrawSize(2.0)
cone.setLimit(30, 45, 170, softness=1.0, bias=0.3, relaxation=8.0)
self.world.attachConstraint(cone)
def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.debugNP.node().showWireframe(True)
self.debugNP.node().showConstraints(True)
self.debugNP.node().showBoundingBoxes(False)
self.debugNP.node().showNormals(False)
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Box A
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
bodyA = BulletRigidBodyNode('Box A')
bodyNP = self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(-2, 0, 4)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box B
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, 0)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Cone
frameA = TransformState.makePosHpr(Point3(0, 0, -2), Vec3(0, 0, 90))
frameB = TransformState.makePosHpr(Point3(-5, 0, 0), Vec3(0, 0, 0))
cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB)
cone.setDebugDrawSize(2.0)
cone.setLimit(30, 45, 170, softness=1.0, bias=0.3, relaxation=8.0)
self.world.attachConstraint(cone)
def _create_joint_constraint(self, joint):
parent_bone = joint.parent_bone
child_bone = joint.child_bone
parent_frame_pos = Vec3(parent_bone.length + joint.gap_radius, 0, 0)
child_frame_pos = Vec3(-child_bone.length - joint.gap_radius, 0, 0)
parent_frame = TransformState.makePosHpr(parent_frame_pos,
Vec3(*joint.parent_start_hpr))
child_frame = TransformState.makePosHpr(child_frame_pos,
Vec3(*joint.child_start_hpr))
constraint = panda3d.bullet.BulletHingeConstraint(
self.bones_to_nodes[parent_bone], self.bones_to_nodes[child_bone],
parent_frame, child_frame)
constraint.setLimit(*joint.angle_range)
constraint.enableFeedback(False)
self.world.attachConstraint(constraint)
self.constraints.append(constraint)
def getPhysBody(self):
bsph = BulletSphereShape(0.6)
bcy = BulletCylinderShape(0.25, 0.35, ZUp)
body = BulletRigidBodyNode('tntBody')
body.addShape(
bsph,
TransformState.makePosHpr((0.05, 0, 0.43),
(86.597, 24.5539, 98.1435)))
body.addShape(
bcy,
TransformState.makePosHpr((0.05, 0.655, 0.35),
(86.597, 24.5539, 98.1435)))
body.setKinematic(True)
body.setCcdMotionThreshold(1e-7)
body.setCcdSweptSphereRadius(0.6)
return body
def testInit(self):
houseId = "0004d52d1aeeb8ae6de39d6bd993e992"
scene = SunCgSceneLoader.loadHouseFromJson(houseId, SUNCG_DATA_DIR)
agent = scene.agents[0]
# Configure the agent
transform = TransformState.makePosHpr(pos=LVector3f(38.42, -39.10, 1.70),
hpr=LVector3f(-77.88, -13.93, 0.0))
agent.setTransform(transform)
renderer = DepthRenderer(scene, size=(512, 512), fov=75.0)
agentId = agent.getTag('agent-id')
image = renderer.getDepthImage(agentId)
fig = plt.figure(figsize=(8, 8))
plt.ion()
plt.show()
plt.axis("off")
plt.imshow(image)
plt.draw()
plt.pause(1.0)
plt.close(fig)
def set_bones_pos_hpr(self, positions, orientations):
# position - n x 3 array
for index, node in enumerate(self._get_ordered_bone_nodes()):
transform = TransformState.makePosHpr(Vec3(*positions[index]),
Vec3(*orientations[index]))
node.setTransform(transform)
node.setLinearVelocity(Vec3(0, 0, 0))
node.setAngularVelocity(Vec3(0, 0, 0))
def rect_region_detection(engine: EngineCore,
position: Tuple,
heading: float,
heading_direction_length: float,
side_direction_width: float,
detection_group: int,
height=10,
in_static_world=False):
"""
----------------------------------
| * | --->>>
----------------------------------
* position
--->>> heading direction
------ longitude length
| lateral width
**CAUTION**: position is the middle point of longitude edge
:param engine: BaseEngine class
:param position: position in PGDrive
:param heading: heading in PGDrive [degree]
:param heading_direction_length: rect length in heading direction
:param side_direction_width: rect width in side direction
:param detection_group: which group to detect
:param height: the detect will be executed from this height to 0
:param in_static_world: execute detection in static world
:return: detection result
"""
region_detect_start = panda_position(position, z=height)
region_detect_end = panda_position(position, z=-1)
tsFrom = TransformState.makePosHpr(region_detect_start,
Vec3(panda_heading(heading), 0, 0))
tsTo = TransformState.makePosHpr(region_detect_end,
Vec3(panda_heading(heading), 0, 0))
shape = BulletBoxShape(
Vec3(heading_direction_length / 2, side_direction_width / 2, 1))
penetration = 0.0
physics_world = engine.physics_world.dynamic_world if not in_static_world else engine.physics_world.static_world
result = physics_world.sweep_test_closest(shape, tsFrom, tsTo,
detection_group, penetration)
return result
def createJoints(self):
for jointDesc in self.joints:
limbA = self.limbs[jointDesc.limbA]
limbB = self.limbs[jointDesc.limbB]
a = self.limbs[jointDesc.limbA].bodyNode
b = self.limbs[jointDesc.limbB].bodyNode
jointA = self.actorJoints[jointDesc.limbA].eNp
jointB = self.actorJoints[jointDesc.limbB].eNp
frame0 = TransformState.makePosHpr(jointDesc.axis0[0], jointDesc.axis0[1])
frame1 = TransformState.makePosHpr(jointDesc.axis1[0], jointDesc.axis1[1])
constraint = BulletConeTwistConstraint(a.node(),
b.node(),
frame0, frame1)
constraint.setLimit(float(jointDesc.swing[0]), float(jointDesc.swing[1]), float(jointDesc.twist))
constraint.setEnabled(True)
constraint.setDebugDrawSize(1.5)
base.physicsWorld.attachConstraint(constraint)
def create_colliders(root, pose_rig, joints_config):
for node, parent in pose_rig.exposed_joints:
if node.getName() not in joints_config:
continue
joint_config = joints_config[node.getName()]
if "joints" not in joint_config:
continue
joints = joint_config["joints"]
if len(joints) == 0:
continue
mass = joint_config["mass"] if "mass" in joint_config else 1
box_rb = BulletRigidBodyNode(node.getName())
box_rb.setMass(1.0)
# box_rb.setLinearDamping(0.2)
# box_rb.setAngularDamping(0.9)
# box_rb.setFriction(1.0)
# box_rb.setAnisotropicFriction(1.0)
# box_rb.setRestitution(0.0)
for joint in joints:
child_node, child_parent = next(
(child_node, child_parent)
for child_node, child_parent in pose_rig.exposed_joints
if child_node.getName() == joint
)
pos = child_node.getPos(child_parent)
width = pos.length() / 2.0
scale = Vec3(3, width, 3)
shape = BulletBoxShape(scale)
quat = Quat()
lookAt(quat, child_node.getPos(child_parent), Vec3.up())
if len(joints) > 1:
transform = TransformState.makePosHpr(child_node.getPos(child_parent) / 2.0, quat.getHpr())
else:
transform = TransformState.makeHpr(quat.getHpr())
box_rb.addShape(shape, transform)
box_np = root.attachNewNode(box_rb)
if len(joints) > 1:
pos = node.getPos(pose_rig.actor)
hpr = node.getHpr(pose_rig.actor)
box_np.setPosHpr(root, pos, hpr)
else:
box_np.setPos(child_parent, child_node.getPos(child_parent) / 2.0)
box_np.lookAt(child_parent, child_node.getPos(child_parent))
yield box_np
def criaPopulacao(self):
vPos = 0
for n in range(0, self.POPULACAO_TREINAMENTO):
indiv = Individuo()
# TRONCO
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
tronco = self.worldNP.attachNewNode(BulletRigidBodyNode('Box'))
tronco.node().setMass(1.0)
tronco.node().addShape(shape)
tronco.setPos(vPos + 2, 0.5, 3)
tronco.setScale(2, 0.6, 4)
tronco.setCollideMask(BitMask32.allOn())
#self.boxNP.node().setDeactivationEnabled(False)
indiv.tronco = tronco
self.world.attachRigidBody(tronco.node())
visualNP = loader.loadModel('models/box.egg')
visualNP.clearModelNodes()
visualNP.reparentTo(tronco)
frameA = TransformState.makePosHpr(Point3(0, 0, -2),
Vec3(0, 0, 90))
# PÉS
shape = BulletBoxShape(Vec3(0.5, 1, 0.2))
posicaoPes = [1, 2]
for i in range(2):
for j in range(1):
x = i + posicaoPes[i] + vPos
y = 0.0
z = j
pe = self.criaPe(shape, x, y, z)
if i == 1:
indiv.pe1 = pe
else:
indiv.pe2 = pe
# Cone
# frameB = TransformState.makePosHpr(Point3(-5, 0, 0), Vec3(0, 0, 0))
# cone = BulletConeTwistConstraint(tronco, pe, frameA, frameB)
# cone.setDebugDrawSize(2.0)
# cone.setLimit(30, 45, 170, softness=1.0, bias=0.3, relaxation=8.0)
# self.world.attachConstraint(cone)
self.individuos.append(indiv)
vPos = vPos + 4
def _create_bone_node(self, bone):
box_shape = panda3d.bullet.BulletBoxShape(
Vec3(bone.length, bone.height, bone.width))
# ts = TransformState.makePos(Point3(bone.length, bone.height, bone.width))
ts = TransformState.makePos(Point3(0, 0, 0))
bone_node = panda3d.bullet.BulletRigidBodyNode(bone.name)
bone_node.setMass(bone.mass)
bone_node.setFriction(bone.friction)
bone_node.addShape(box_shape, ts)
bone_node.setTransform(
TransformState.makePosHpr(Vec3(*bone.start_pos),
Vec3(*bone.start_hpr)))
self.world.attachRigidBody(bone_node)
self.bones_to_nodes[bone] = bone_node
def createLimbs(self):
# For each limbs desc, create a bullet capsule
for limb in self.limbs.values():
body = BulletRigidBodyNode("ragdoll-limb-" + limb.jointName)
body.setMass(limb.mass)
for i in range(len(limb.shapes)):
shapeDesc = limb.shapes[i]
capsule = BulletCapsuleShape(shapeDesc.radius, shapeDesc.length / 2.0, ZUp)
body.addShape(capsule, TransformState.makePosHpr(shapeDesc.localPos, shapeDesc.localHpr))
jointDesc = self.actorJoints[limb.jointName]
eNp = jointDesc.eNp
jointDesc.limb = limb
bodyNp = NodePath(body)
bodyNp.reparentTo(render)
bodyNp.setTransform(render, eNp.getTransform(render))
bodyNp.node().setTransformDirty()
base.physicsWorld.attachRigidBody(body)
limb.bodyNode = bodyNp
def insert(self, world, render, i, pos):
# Important numbers
head_radius = 0.5
head_elevation = 1.5
torso_x = 0.3
torso_y = 0.5
torso_z = 0.75
arm_radius = 0.15
shoulder_space = 0.05
shoulder_elevation = head_elevation - head_radius - 0.1 - arm_radius
torso_elevation = head_elevation - head_radius - torso_z
x, y = pos
# measurements below are in degrees
neck_yaw_limit = 90
neck_pitch_limit = 45
shoulder_twist_limit = 90 # limit for twisting arm along the bicep axis
shoulder_in_limit = 175 # maximum declination from T-pose towards torso
shoulder_out_limit = 90 # maximum elevation from T-pose away from torso
shoulder_forward_limit = 175 # maximum angle from down by side to pointing forward
shoulder_backward_limit = 90 # maximum angle from down by side to pointing backward
# Create a head
head_node = BulletRigidBodyNode('Head')
head_node.addShape(BulletSphereShape(head_radius))
head_node.setMass(1.0)
head_pointer = render.attachNewNode(head_node)
head_pointer.setPos(x, y, head_elevation)
world.attachRigidBody(head_node)
# Create a torso
torso_node = BulletRigidBodyNode('Torso')
torso_node.addShape(BulletBoxShape(Vec3(torso_x, torso_y, torso_z)))
torso_node.setMass(0.0) # remain in place
torso_pointer = render.attachNewNode(torso_node)
torso_pointer.setPos(x, y, torso_elevation)
world.attachRigidBody(torso_node)
# Attach the head to the torso
head_frame = TransformState.makePosHpr(Point3(0, 0, -head_radius),
Vec3(0, 0, -90))
torso_frame = TransformState.makePosHpr(Point3(0, 0, torso_z),
Vec3(0, 0, -90))
neck = BulletConeTwistConstraint(head_node, torso_node, head_frame,
torso_frame)
neck.setDebugDrawSize(0.5)
neck.setLimit(neck_pitch_limit, neck_pitch_limit, neck_yaw_limit)
world.attachConstraint(neck)
# Create arms
shoulder_pos_l = Point3(
x, y - i * (torso_y + shoulder_space + arm_radius),
shoulder_elevation)
shoulder_pos_r = Point3(
x, y + i * (torso_y + shoulder_space + arm_radius),
shoulder_elevation)
limits = (shoulder_in_limit, shoulder_out_limit,
shoulder_forward_limit, shoulder_backward_limit,
shoulder_twist_limit)
arm_l = Arm(world, render, shoulder_pos_l, -i, LEFT, torso_pointer,
limits)
arm_r = Arm(world, render, shoulder_pos_r, i, RIGHT, torso_pointer,
limits)
self.head = head_pointer
self.torso = torso_pointer
self.arm_l, self.arm_r = arm_l, arm_r
def main():
# Create scene and remove any default agents
scene = SunCgSceneLoader.loadHouseFromJson(
houseId="0004d52d1aeeb8ae6de39d6bd993e992", datasetRoot=SUNCG_DATA_DIR)
scene.scene.find('**/agents').node().removeAllChildren()
scene.agents = []
# Create multiple agents
agents = []
for i in range(3):
agentRadius = 0.15
agent = Agent(scene, 'agent-%d' % (i), agentRadius)
agents.append(agent)
# NOTE: specify to move the camera slightly outside the model (not to render the interior of the model)
cameraTransform = TransformState.makePos(
LVector3f(0.0, 1.1 * agentRadius, 0.0))
# Initialize rendering and physics
renderer = RgbRenderer(scene,
size=(128, 128),
fov=70.0,
cameraTransform=cameraTransform)
renderer.showRoomLayout(showCeilings=False,
showWalls=True,
showFloors=True)
physics = Panda3dBulletPhysics(scene,
SUNCG_DATA_DIR,
objectMode='box',
agentRadius=agentRadius,
agentMode='sphere')
# Configure the camera
viewer = Viewer(scene,
interactive=False,
showPosition=False,
cameraMask=renderer.cameraMask)
transform = TransformState.makePosHpr(LVecBase3f(44.01, -43.95, 22.97),
LVecBase3f(0.0, -81.04, 0.0))
viewer.cam.setTransform(transform)
# Initialize the agent
agents[0].setPosition((45, -42.5, 1.6))
agents[1].setPosition((42.5, -39, 1.6))
agents[2].setPosition((42.5, -41.5, 1.6))
# Initialize figure that will show the point-of-view of each agent
plt.ion()
fig = plt.figure(figsize=(12, 4), facecolor='white')
ims = []
for i in range(len(agents)):
ax = fig.add_subplot(1, len(agents), i + 1)
ax.set_title(agents[i].agentId)
ax.axis('off')
rgbImage = np.zeros(renderer.size + (3, ), dtype=np.uint8)
im = ax.imshow(rgbImage)
ims.append(im)
plt.tight_layout()
plt.show()
# Main loop
clock = ClockObject.getGlobalClock()
try:
while True:
# Update physics
dt = clock.getDt()
physics.step(dt)
# Update viewer
viewer.step()
for i, agent in enumerate(agents):
# Get the current RGB
rgbImage = renderer.getRgbImage(agent.agentId,
channelOrder="RGB")
# Get the current observation for the agent
observation = {
"position": agent.getPosition(),
"orientation": agent.getOrientation(),
"rgb-image": rgbImage
}
agent.step(observation)
ims[i].set_data(rgbImage)
fig.canvas.draw()
plt.pause(0.0001)
except KeyboardInterrupt:
pass
viewer.destroy()
renderer.destroy()
return 0
def setup(self):
#self.targetAlt = r.randrange(100,300)
self.Valves = np.array([0.15, 0.2, 0.15])
self.EngObs = self.vulcain.predict_data_point(
np.array(self.Valves).reshape(1, -1))
if self.VISUALIZE is True:
self.worldNP = self.render.attachNewNode('World')
else:
self.root = NodePath(PandaNode("world root"))
self.worldNP = self.root.attachNewNode('World')
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.80665))
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.node().showWireframe(True)
self.debugNP.node().showConstraints(True)
self.debugNP.node().showBoundingBoxes(False)
self.debugNP.node().showNormals(True)
self.debugNP.show()
self.world.setDebugNode(self.debugNP.node())
# self.debugNP.showTightBounds()
# self.debugNP.showBounds()
# Ground (static)
shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
self.groundNP = self.worldNP.attachNewNode(
BulletRigidBodyNode('Ground'))
self.groundNP.node().addShape(shape)
self.groundNP.setPos(0, 0, 0)
self.groundNP.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(self.groundNP.node())
# Rocket
shape = BulletCylinderShape(self.radius, self.length, ZUp)
self.rocketNP = self.worldNP.attachNewNode(
BulletRigidBodyNode('Cylinder'))
self.rocketNP.node().setMass(self.drymass + self.fuelAmount_LH2 +
self.fuelAmount_LOX)
self.rocketNP.node().addShape(shape)
#self.rocketNP.setPos(20,20,250)
self.rocketNP.setPos(r.randrange(-200, 200), 20,
350) #r.randrange(300, 500))
#self.rocketNP.setPos(r.randrange(-self.lateralError, self.lateralError, 1), r.randrange(-self.lateralError, self.lateralError, 1), self.height)
# self.rocketNP.setPos(0, 0, self.length*10)
self.rocketNP.setCollideMask(BitMask32.allOn())
# self.rocketNP.node().setCollisionResponse(0)
self.rocketNP.node().notifyCollisions(True)
self.world.attachRigidBody(self.rocketNP.node())
for i in range(4):
leg = BulletCylinderShape(0.1 * self.radius, 0.5 * self.length,
XUp)
self.rocketNP.node().addShape(
leg,
TransformState.makePosHpr(
Vec3(6 * self.radius * math.cos(i * math.pi / 2),
6 * self.radius * math.sin(i * math.pi / 2),
-0.6 * self.length), Vec3(i * 90, 0, 30)))
shape = BulletConeShape(0.75 * self.radius, 0.5 * self.radius, ZUp)
self.rocketNP.node().addShape(
shape,
TransformState.makePosHpr(Vec3(0, 0, -1 / 2 * self.length),
Vec3(0, 0, 0)))
self.npThrustForce = LineNodePath(self.rocketNP,
'Thrust',
thickness=4,
colorVec=Vec4(1, 0.5, 0, 1))
self.npDragForce = LineNodePath(self.rocketNP,
'Drag',
thickness=4,
colorVec=Vec4(1, 0, 0, 1))
self.npLiftForce = LineNodePath(self.rocketNP,
'Lift',
thickness=4,
colorVec=Vec4(0, 0, 1, 1))
self.rocketCSLon = self.radius**2 * math.pi
self.rocketCSLat = self.length * 2 * self.radius
if self.VISUALIZE is True:
self.terrain = loader.loadModel("../LZGrid2.egg")
self.terrain.setScale(10)
self.terrain.reparentTo(self.render)
self.terrain.setColor(Vec4(0.1, 0.2, 0.1, 1))
self.toggleTexture()
def __init__(self,info):
# animation base setup
self.character_info_ = info
size = Vec3(info.width, AnimationActor.DEFAULT_WIDTH , info.height)
AnimatableObject.__init__(self,info.name,size,info.mass)
self.animation_root_np_.setPos(Vec3(0,0,0))
# constraints
self.left_constraint_ = None
self.right_constraint_ = None
self.selected_constraint_ = None
# removing default shapes from rigid body
shapes = self.node().getShapes()
for shape in shapes:
self.node().removeShape(shape)
# adding capsule shape
radius = 0.5*size.getX()
height = size.getZ() - 2.0*radius # height of cylindrical part only
height = height if height >= 0 else 0.0
bullet_shape = BulletCapsuleShape(radius, height, ZUp)
bullet_shape.setMargin(GameObject.DEFAULT_COLLISION_MARGIN)
self.node().addShape(bullet_shape,TransformState.makePos(Vec3(0,0,0.5*size.getZ()))) # box bottom center coincident with the origin
self.node().setMass(self.character_info_.mass)
self.node().setLinearFactor((1,1,1))
self.node().setAngularFactor((0,0,0))
self.setCollideMask(CollisionMasks.NO_COLLISION)
# setting bounding volume
min = LPoint3(-0.5*size.getX(),-0.5*size.getY(),0)
max = LPoint3(0.5*size.getX(),0.5*size.getY(),size.getZ())
self.node().setBoundsType(BoundingVolume.BT_box)
self.node().setBounds(BoundingBox(min,max))
# setting origin ghost nodes
rel_pos = Vec3(-GameObject.ORIGIN_XOFFSET,0,info.height/2)
self.left_origin_gn_ = self.attachNewNode(BulletGhostNode(self.getName() + '-left-origin'))
self.left_origin_gn_.node().addShape(BulletSphereShape(GameObject.ORIGIN_SPHERE_RADIUS),TransformState.makePosHpr(rel_pos,Vec3.zero()))
self.left_origin_gn_.node().setIntoCollideMask(CollisionMasks.ACTION_TRIGGER_0 if not self.isFacingRight() else CollisionMasks.NO_COLLISION)
rel_pos = Vec3(GameObject.ORIGIN_XOFFSET,0,info.height/2)
self.right_origin_gn_ = self.attachNewNode(BulletGhostNode(self.getName() + '-right-origin'))
self.right_origin_gn_.node().addShape(BulletSphereShape(GameObject.ORIGIN_SPHERE_RADIUS),TransformState.makePosHpr(rel_pos,Vec3.zero()))
self.right_origin_gn_.node().setIntoCollideMask(CollisionMasks.ACTION_TRIGGER_0 if self.isFacingRight() else CollisionMasks.NO_COLLISION)
# character status
self.state_data_ = CharacterStateData()
# state machine
self.sm_ = StateMachine()
# motion commander
self.motion_commander_ = MotionCommander(self)
# set id
self.setObjectID(self.getName())
def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.debugNP.node().showWireframe(True)
self.debugNP.node().showConstraints(True)
self.debugNP.node().showBoundingBoxes(False)
self.debugNP.node().showNormals(True)
#self.debugNP.showTightBounds()
#self.debugNP.showBounds()
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Ground (static)
shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
self.groundNP = self.worldNP.attachNewNode(
BulletRigidBodyNode('Ground'))
self.groundNP.node().addShape(shape)
self.groundNP.setPos(0, 0, -2)
self.groundNP.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(self.groundNP.node())
#Rocket
shape = BulletCylinderShape(0.2 * self.scale, 2 * self.scale, ZUp)
self.rocketNP = self.worldNP.attachNewNode(
BulletRigidBodyNode('Cylinder'))
self.rocketNP.node().setMass(3.0)
self.rocketNP.node().addShape(shape)
self.rocketNP.setPos(0, 0, 2 * self.scale)
self.rocketNP.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(self.rocketNP.node())
for i in range(4):
leg = BulletCylinderShape(0.02 * self.scale, 1 * self.scale, XUp)
self.rocketNP.node().addShape(
leg,
TransformState.makePosHpr(
Vec3(0.6 * self.scale * math.cos(i * math.pi / 2),
0.6 * self.scale * math.sin(i * math.pi / 2),
-1.2 * self.scale), Vec3(i * 90, 0, 30)))
shape = BulletConeShape(0.15 * self.scale, 0.3 * self.scale, ZUp)
self.rocketNozzle = self.worldNP.attachNewNode(
BulletRigidBodyNode('Cone'))
self.rocketNozzle.node().setMass(1)
self.rocketNozzle.node().addShape(shape)
self.rocketNozzle.setPos(0, 0, 0.8 * self.scale)
self.rocketNozzle.setCollideMask(BitMask32.allOn())
self.rocketNozzle.node().setCollisionResponse(0)
self.world.attachRigidBody(self.rocketNozzle.node())
frameA = TransformState.makePosHpr(Point3(0, 0, -1 * self.scale),
Vec3(0, 0, 90))
frameB = TransformState.makePosHpr(Point3(0, 0, 0.2 * self.scale),
Vec3(0, 0, 90))
self.cone = BulletConeTwistConstraint(self.rocketNP.node(),
self.rocketNozzle.node(), frameA,
frameB)
self.cone.enableMotor(1)
#self.cone.setMaxMotorImpulse(2)
#self.cone.setDamping(1000)
self.cone.setDebugDrawSize(2.0)
self.cone.setLimit(20, 20, 0, softness=1.0, bias=1.0, relaxation=10.0)
self.world.attachConstraint(self.cone)
self.npThrustForce = LineNodePath(self.render,
'Thrust',
thickness=4,
colorVec=VBase4(1, 0.5, 0, 1))
def create_constraints(root, joint_pairs, offset_scale):
for joint_config, parent, child in joint_pairs:
rb_parent = parent.node()
rb_child = child.node()
extents_parent = rb_parent.get_shape(0).getHalfExtentsWithMargin()
extents_child = rb_child.get_shape(0).getHalfExtentsWithMargin()
if "offset_parent" in joint_config:
offset_parent = Point3(joint_config["offset_parent"])
else:
offset_parent = (0, 1, 0)
offset_parent_x, offset_parent_y, offset_parent_z = offset_parent
offset_parent = Point3(
offset_parent_x * extents_parent.getX(),
offset_parent_y * extents_parent.getY(),
offset_parent_z * extents_parent.getZ(),
)
if "offset_child" in joint_config:
offset_child = Point3(*joint_config["offset_child"])
else:
offset_child = (0, -1, 0)
offset_child_x, offset_child_y, offset_child_z = offset_child
offset_child = Point3(
offset_child_x * extents_child.getX(),
offset_child_y * extents_child.getY(),
offset_child_z * extents_child.getZ(),
)
offset_parent = offset_parent * offset_scale
offset_child = offset_child * offset_scale
if joint_config["type"] == "hinge":
axis_parent = Vec3(*joint_config["axis_parent"])
axis_child = Vec3(*joint_config["axis_child"])
constraint = BulletHingeConstraint(
rb_parent, rb_child, offset_parent, offset_child, axis_parent, axis_child
)
if "limit" in joint_config:
low, high = joint_config["limit"]
softness = 1.0
bias = 0.3
relaxation = 1.0
constraint.setLimit(low, high, softness, bias, relaxation)
elif joint_config["type"] == "cone":
frame_parent = TransformState.makePosHpr(offset_parent, Vec3(90, 0, 0))
frame_child = TransformState.makePosHpr(offset_child, Vec3(90, 0, 0))
constraint = BulletConeTwistConstraint(rb_parent, rb_child, frame_parent, frame_child)
swing1, swing2, twist = joint_config["limit"]
constraint.setLimit(swing1, swing2, twist)
elif joint_config["type"] == "spherical":
constraint = BulletSphericalConstraint(rb_parent, rb_child, offset_parent, offset_child)
# constraint.setDebugDrawSize(1.0)
yield constraint
def setupObstacleFour(self, pos, scale, turn):
#Start Here
# Box A
shape = BulletBoxShape(Vec3(0.01, 0.01, 0.01) * scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP = self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 4) #(0, 0, 4)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.01, 0.01, 0.01)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box B
shape = BulletSphereShape(0.5*scale)
bodyB = BulletRigidBodyNode('Sphere B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(10.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 5) #(0, 0, 0.001)
visNP = loader.loadModel('media/models/ball.egg')
visNP.clearModelNodes()
visNP.setScale(1.25*scale)
visNP.reparentTo(bodyNP)
bodyNP.node().setLinearVelocity(100)
self.world.attachRigidBody(bodyB)
# Cone
frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90))
frameB = TransformState.makePosHpr(Point3(2, 0, 0)*scale, Vec3(0, 0, 0))
cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB)
cone.setDebugDrawSize(2.0)
cone.setLimit(30, 90, 270, softness=1.0, bias=0.3, relaxation=10.0)
self.world.attachConstraint(cone)
# Box C
shape = BulletBoxShape(Vec3(0.1, 0.1, 1)*scale)
bodyC = BulletRigidBodyNode('Box C')
bodyNP = self.worldNP.attachNewNode(bodyC)
bodyNP.node().addShape(shape)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 3)
self.world.attachRigidBody(bodyC)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 0.1, 1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
def setup(self):
self.targetAlt = r.randrange(100,300)
self.Valves = np.array([0.15,0.2,0.15])
self.EngObs = self.vulcain.predict_data_point(np.array(self.Valves).reshape(1,-1))
if self.VISUALIZE is True:
self.worldNP = self.render.attachNewNode('World')
else:
self.root = NodePath(PandaNode("world root"))
self.worldNP = self.root.attachNewNode('World')
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.80665))
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.node().showWireframe(True)
self.debugNP.node().showConstraints(True)
self.debugNP.node().showBoundingBoxes(False)
self.debugNP.node().showNormals(True)
self.debugNP.show()
self.world.setDebugNode(self.debugNP.node())
# self.debugNP.showTightBounds()
# self.debugNP.showBounds()
# Ground (static)
shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
self.groundNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
self.groundNP.node().addShape(shape)
self.groundNP.setPos(0, 0, 0)
self.groundNP.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(self.groundNP.node())
# Rocket
shape = BulletCylinderShape(self.radius, self.length, ZUp)
self.rocketNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Cylinder'))
self.rocketNP.node().setMass(27200 * self.scale)
self.rocketNP.node().addShape(shape)
#self.rocketNP.setPos(20,20,250)
self.rocketNP.setPos(20, 20, r.randrange(100, 300)+200)
#self.rocketNP.setPos(r.randrange(-self.lateralError, self.lateralError, 1), r.randrange(-self.lateralError, self.lateralError, 1), self.height)
# self.rocketNP.setPos(0, 0, self.length*10)
self.rocketNP.setCollideMask(BitMask32.allOn())
# self.rocketNP.node().setCollisionResponse(0)
self.rocketNP.node().notifyCollisions(True)
self.world.attachRigidBody(self.rocketNP.node())
for i in range(4):
leg = BulletCylinderShape(0.1 * self.radius, 0.5 * self.length, XUp)
self.rocketNP.node().addShape(leg, TransformState.makePosHpr(
Vec3(6 * self.radius * math.cos(i * math.pi / 2), 6 * self.radius * math.sin(i * math.pi / 2),
-0.6 * self.length), Vec3(i * 90, 0, 30)))
shape = BulletConeShape(0.75 * self.radius, 0.5 * self.radius, ZUp)
self.rocketNP.node().addShape(shape, TransformState.makePosHpr(Vec3(0, 0, -1 / 2 * self.length), Vec3(0, 0, 0)))
# Fuel
self.fuelRadius = 0.9 * self.radius
shape = BulletCylinderShape(self.fuelRadius, 0.01 * self.length, ZUp)
self.fuelNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Cone'))
self.fuelNP.node().setMass(self.fuelMass_full * self.fuelMass_init)
self.fuelNP.node().addShape(shape)
self.fuelNP.setPos(0, 0, self.rocketNP.getPos().getZ() - self.length * 0.5 * (1 - self.fuelMass_init))
self.fuelNP.setCollideMask(BitMask32.allOn())
self.fuelNP.node().setCollisionResponse(0)
self.world.attachRigidBody(self.fuelNP.node())
frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90))
frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90))
self.fuelSlider = BulletSliderConstraint(self.rocketNP.node(), self.fuelNP.node(), frameA, frameB, 1)
self.fuelSlider.setTargetLinearMotorVelocity(0)
self.fuelSlider.setDebugDrawSize(2.0)
self.fuelSlider.set_lower_linear_limit(0)
self.fuelSlider.set_upper_linear_limit(0)
self.world.attachConstraint(self.fuelSlider)
self.npThrustForce = LineNodePath(self.rocketNP, 'Thrust', thickness=4, colorVec=Vec4(1, 0.5, 0, 1))
self.npDragForce = LineNodePath(self.rocketNP, 'Drag', thickness=4, colorVec=Vec4(1, 0, 0, 1))
self.npLiftForce = LineNodePath(self.rocketNP, 'Lift', thickness=4, colorVec=Vec4(0, 0, 1, 1))
self.npFuelState = LineNodePath(self.fuelNP, 'Fuel', thickness=20, colorVec=Vec4(0, 1, 0, 1))
self.rocketCSLon = self.radius ** 2 * math.pi
self.rocketCSLat = self.length * 2 * self.radius
if self.VISUALIZE is True:
self.terrain = loader.loadModel("LZGrid2.egg")
self.terrain.setScale(10)
self.terrain.reparentTo(self.render)
self.terrain.setColor(Vec4(0.1, 0.2, 0.1, 1))
self.toggleTexture()
#self.fuelNP.setPos(0, 0, self.rocketNP.getPos().getZ() - self.length * 0.4 * (1 - self.fuelMass_init))
for i in range(5):
self.world.doPhysics(self.dt, 5, 1.0 / 180.0)
self.fuelSlider.set_lower_linear_limit(-self.length * 0.5 * (1 - self.fuelMass_init))
self.fuelSlider.set_upper_linear_limit(self.length * 0.5 * (1 - self.fuelMass_init))
for i in range(100):
self.world.doPhysics(self.dt, 5, 1.0 / 180.0)
self.rocketNP.node().applyForce(Vec3(0,0,300000), Vec3(0, 0, 0))
def setupObstacleThree(self, pos, scale, turn):
# Box A
shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1))
bodyA = BulletRigidBodyNode('Box A')
bodyA.setRestitution(1.0)
bodyNP = self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 0.1, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
#Box B
shape = BulletBoxShape(Vec3(0.1,0.1,0.1))
bodyB = BulletRigidBodyNode('Box B')
bodyB.setRestitution(1.0)
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1,0.1,0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Slider
frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True)
slider.setDebugDrawSize(2.0)
slider.setLowerLinearLimit(0)
slider.setUpperLinearLimit(12)
slider.setLowerAngularLimit(-90)
slider.setUpperAngularLimit(-85)
self.world.attachConstraint(slider)
# Box C
shape = BulletBoxShape(Vec3(1, 3, 0.1))
bodyC = BulletRigidBodyNode('Box C')
bodyC.setRestitution(1.0)
bodyNP = self.worldNP.attachNewNode(bodyC)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(0.1)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(Vec3(pos.getX() + 3, pos.getY() - 4, pos.getZ()))
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(1, 3, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyC)
bodyNP.node().setLinearVelocity(-100)
# Slider
frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
slider = BulletSliderConstraint(bodyA, bodyC, frameA, frameB, True)
slider.setDebugDrawSize(2.0)
slider.setLowerLinearLimit(2)
slider.setUpperLinearLimit(6)
slider.setLowerAngularLimit(-90)
slider.setUpperAngularLimit(-85)
self.world.attachConstraint(slider)
