def __init__(self, settings, ID, actor=None):
self.id = ID
self.actor = actor
self.rotation_speed = settings.agent_rotation_speed
self.force = settings.agent_force
self._color = b2Color(0.4, 0.4, 0.6)
self.color = b2Color(0.4, 0.4, 0.6)
def color(self):
if self.team == 0:
return b2Color(0.2, 0.2, 1)
if self.team == 1:
return b2Color(1, 0.2, 0.2)
if self.team == 2:
return b2Color(0.2, 1, 0.2)
def __init__(self, container, width, height):
SimulationBase.__init__(self, container, width, height)
# --- pybox2d world setup ---
# Create the world
self.m_b2dWorld = world(gravity=(0, -10), doSleep=True)
self.m_debugDraw = DebugDrawExtended(surface=settings.OBJ_SURFACE)
self.m_b2dWorld.renderer = self.m_debugDraw
self.m_b2dWorld.warmStarting = True
self.m_b2dWorld.continuousPhysics = True
self.m_b2dWorld.subStepping = False
self.m_groundBody = None
self.mouseJoint = None
self.colours = {
'mouse_point': b2Color(0, 1, 0),
'joint_line': b2Color(0.8, 0.8, 0.8)
}
self.m_joints = []
self.init()
self.setupWorld()
def Step(self, settings):
# PREDICT
# qval = None
# fixing = False
translating = False
if self.car.crashed:
for obstacle in self.obstacles:
obstacle.fixtures[0].sensor = True
self.car.body.transform = (self.car.body.position * -1, 0)
self.car.body.angle = math.radians(np.random.randint(0, 359))
translating = True
else:
for obstacle in self.obstacles:
obstacle.fixtures[0].sensor = False
self.steps += 1
self.pressed_keys = set()
action = self.learner.take_action(self.steps)
if action == 0:
pass
elif action == 1:
self.pressed_keys.add('left')
elif action == 2:
self.pressed_keys.add('right')
# UPDATE
was_crashed = self.car.crashed
was_hitting_goal = self.car.hit_goal
self.car.update(self.pressed_keys, settings.hz, self.world, self.goal,
self.goal_radius)
super(TopDownCar, self).Step(settings)
just_crashed = not was_crashed and self.car.crashed
just_hit_goal = not was_hitting_goal and self.car.hit_goal
if translating:
body_angle = self.car.body.angle
for tire in self.car.tires:
tire.body.angle = body_angle
self.learner.reinforce(self.steps, just_crashed, just_hit_goal,
self.goal.position)
# easy just count number of goals hit
# if self.steps % 15000 == 0:
# DRAW
self.renderer.DrawPoint(self.renderer.to_screen(self.car.lidar), 5.0,
b2Color(0.4, 0.9, 0.4))
for pt in self.car.sensor_tips:
self.renderer.DrawSegment(self.renderer.to_screen(self.car.lidar),
self.renderer.to_screen(pt),
b2Color(0.4, 0.9, 0.4))
for dist, pt in self.car.sensor_readings:
if dist != self.car.sensor_len:
self.renderer.DrawPoint(self.renderer.to_screen(pt), 5.0,
b2Color(0.4, 0.9, 0.4))
def _print_observations_to_screen(self):
robot_obs_dict = \
self._friendly_robots_handler.get_observations()
if not robot_obs_dict:
return
neg_ecores, pos_ecores = self._ecores_handler.get_ecore_counts()
results_str = f'Neg Ecores: {neg_ecores} | Pos Ecores: {pos_ecores}\n'
aruco_id = next(iter(robot_obs_dict))
results_str = \
results_str + f'Lower observations for robot: {aruco_id}\n'
results_str = results_str + \
f'{"|".join(self._friendly_robots_handler.lower_tags)}\n'
angles = [
element * 180 / b2_pi
for element in self._friendly_robots_handler.angles
]
results_str = results_str + print_observations(
robot_obs_dict[aruco_id]['lower_obs'],
angles,
return_string=True,
include_raw=False)
results_str_arr = results_str.split("\n")
y_start = 230
for line in results_str_arr:
self.DrawStringAt(30, y_start, line, b2Color(0, 0, 0))
y_start += 18
def laser_display(renderer, laser_body, length=30.0, laser_color=(1, 0, 0), laser_half_width=2, **kwargs):
if not renderer:
return
p1, p2 = get_laser_line(laser_body, length, laser_half_width)
renderer.DrawSegment(renderer.to_screen(
p1), renderer.to_screen(p2), b2Color(*laser_color))
def __init__(self,
n_agents=[10],
actors=None,
colors=None,
targets=None,
**kwargs):
self.settings = flockSettings(**kwargs)
self.framework = PygletFramework(
self.settings) if self.settings.render else NoRender(self.settings)
self.framework.env = self
self.done = False
self.n_agents = n_agents
self.n_targets = 1 if targets == None else len(np.unique(targets))
self.targets_idx = [0] * n_agents[0] if targets == None else targets
self.time_passed = 0
# create random target location
self.targets = []
for t in range(self.n_targets):
self.t_min, self.t_max = self.settings.target_mindist, self.settings.target_maxdist
rand_angle = 2 * np.pi * random.random()
rand_dist = self.t_min + random.random() * (self.t_max -
self.t_min)
self.targets.append(
b2Vec2(rand_dist * np.cos(rand_angle),
rand_dist * np.sin(rand_angle)))
if self.settings.render:
self.framework.gui_objects["target" + str(t)] = {
'shape':
'circle',
'values': [
self.targets[t], self.settings.reward_radius,
b2Color(1, 1, 1)
]
}
self.selected_target = 0
# create agents
self.agents = []
for i in range(sum(self.n_agents)):
x = self.settings.start_spread * (
random.random() - 0.5) + self.settings.start_point[0]
y = self.settings.start_spread * (
random.random() - 0.5) + self.settings.start_point[1]
angle = random.uniform(-1, 1) * np.pi
agent = Agent(self.settings, ID=i)
if actors:
agent.actor = actors[i]
if colors:
agent._color = colors[i]
agent.body = self.framework.world.CreateDynamicBody(
**self.settings.bodySettings,
position=(x, y),
angle=angle,
userData=agent)
self.agents.append(agent)
self.create_space()
self.create_space_flag = False
self.obs = self.get_obs()
def __init__(self, world, renderer, params):
self._params = params
self.p1_color = b2Color(0.4, 0.9, 0.4)
# self.s1_color = b2Color(0.8, 0.8, 0.8)
# self.s2_color = b2Color(0.9, 0.9, 0.4)
self._line_colors = [
b2Color(0.0, 0.0, 1.0),
b2Color(0.0, 1.0, 0.0),
b2Color(1.0, 0.0, 0.0)
]
self._front_line_color = b2Color(0.0, 1.0, 1.0)
self.world = world
self.renderer = renderer
self.ray_angles = get_ray_angles(
2 * b2_pi / 360 * params["image_processing"]["max_angle_per_side"],
self._params["image_processing"]["number_of_rays_per_side"])
def MouseDown(self, p):
"""
Mouse moved to point p, in world coordinates.
"""
self.targets[self.selected_target] = p
self.framework.gui_objects["target" + str(self.selected_target)] = {
'shape': 'circle',
'values': [p, self.settings.reward_radius,
b2Color(1, 1, 1)]
}
def test_color(self):
x, y, z = 1.0, 2.0, 3.0
c1 = b2Color(x, y, z)
c2 = b2Color(z, y, x)
c = c1 + c2
self.checkAlmostEqual(c, (x + z, y + y, z + x), msg='b2Color +')
c = c1 - c2
self.checkAlmostEqual(c, (x - z, y - y, z - x), msg='b2Color -')
c = 2.0 * c1
self.checkAlmostEqual(c, (x + x, y + y, z + z), msg='float * b2Color')
c = c1 * 2.0
self.checkAlmostEqual(c, (x + x, y + y, z + z), msg='b2Color * float')
c = c1 / 2.0
self.checkAlmostEqual(c, (x / 2.0, y / 2.0, z / 2.0),
msg='b2Color / float')
c = c1.copy()
c *= 2.0
self.checkAlmostEqual(c, (x + x, y + y, z + z), msg='b2Color *= float')
c = b2Color(c1)
c /= 2.0
self.checkAlmostEqual(c, (x / 2.0, y / 2.0, z / 2.0),
msg='b2Color /= float')
c1 += (1.0, 1.0, 1.0)
self.checkAlmostEqual(c1, (x + 1, y + 1, z + 1), msg='b2Color +=')
c1 -= (1.0, 1.0, 1.0)
self.checkAlmostEqual(c1, (x, y, z), msg='b2Color -=')
bytes = b2Color(1, 1, 1).bytes
self.assertEqual(bytes, [255, 255, 255],
msg='bytes (1,1,1)=>%s' % bytes)
bytes = b2Color(0, 0, 0).bytes
self.assertEqual(bytes, [0, 0, 0], msg='bytes (1,1,1)=>%s' % bytes)
self.assertEqual(c1[0], x)
self.assertEqual(c1[1], y)
self.assertEqual(c1[2], z)
c1.list = (x * 2, y * 2, z * 2)
self.checkAlmostEqual(c1, (x + x, y + y, z + z), msg='b2Color.list')
def get_rewards(self):
rewards = {}
for contact in self.framework.world.contacts:
rewards[contact.fixtureA.body.userData.id] = -1
rewards[contact.fixtureB.body.userData.id] = -1
if self.settings.render:
contact.fixtureA.body.userData.color = b2Color(1, 0.2, 0.2)
contact.fixtureB.body.userData.color = b2Color(1, 0.2, 0.2)
for agent in self.agents:
if agent.id not in rewards:
d = np.sqrt(
b2DistanceSquared(self.targets[self.targets_idx[agent.id]],
agent.body.position))
if self.settings.reward_mode == "linear":
rewards[agent.id] = (-d / 35) + 1 # arbitrary...
if self.settings.render:
agent.reset_color()
if self.settings.reward_mode == "binary":
r = int(d < self.settings.reward_radius)
rewards[agent.id] = r
return rewards
def laser_display(renderer,
laser_body,
length=30.0,
laser_color=(1, 0, 0),
laser_half_width=2,
**kwargs):
if not renderer:
return
p1, p2 = get_laser_line(laser_body, length, laser_half_width)
renderer.DrawSegment(renderer.to_screen(p1), renderer.to_screen(p2),
b2Color(*laser_color))
def ManualDraw(self):
"""
This implements code normally present in the C++ version,
which calls the callbacks that you see in this class (DrawSegment,
DrawSolidCircle, etc.).
This is implemented in Python as an example of how to do it, and also
a test.
"""
colors = {
'active': b2Color(0.5, 0.5, 0.3),
'static': b2Color(0.5, 0.9, 0.5),
'kinematic': b2Color(0.5, 0.5, 0.9),
'asleep': b2Color(0.6, 0.6, 0.6),
'default': b2Color(0.9, 0.7, 0.7),
}
if self.test.settings.drawShapes:
for body in self.test.world.bodies:
transform = body.transform
color = body.userData.color if body.userData else colors[
"default"]
if not body.active:
color = color / 3
for fixture in body.fixtures:
# if not body.active:
# color = colors['active']
# elif body.type == b2_staticBody:
# color = colors['static']
# elif body.type == b2_kinematicBody:
# color = colors['kinematic']
# elif not body.awake:
# color = colors['asleep']
# else:
# color = colors['default']
self.DrawBodyShape(fixture.shape, transform, color)
for object in self.test.gui_objects.values():
if object['shape'] == 'circle':
self.DrawCircle(object['values'][0], object['values'][1],
object['values'][2])
# if self.test.settings.drawJoints:
# for joint in self.test.world.joints:
# self.DrawJoint(joint)
if self.test.settings.drawAABBs:
color = b2Color(0.9, 0.3, 0.9)
cm = self.test.world.contactManager
for body in self.test.world.bodies:
if not body.active:
continue
transform = body.transform
for fixture in body.fixtures:
shape = fixture.shape
for childIndex in range(shape.childCount):
self.DrawAABB(shape.getAABB(transform, childIndex),
color)
def test_color(self):
x, y, z = 1.0, 2.0, 3.0
c1 = b2Color(x, y, z)
c2 = b2Color(z, y, x)
c = c1 + c2
self.checkAlmostEqual(c, (x+z, y+y, z+x), msg='b2Color +')
c = c1 - c2
self.checkAlmostEqual(c, (x-z, y-y, z-x), msg='b2Color -')
c = 2.0 * c1
self.checkAlmostEqual(c, (x+x, y+y, z+z), msg='float * b2Color')
c = c1 * 2.0
self.checkAlmostEqual(c, (x+x, y+y, z+z), msg='b2Color * float')
c = c1 / 2.0
self.checkAlmostEqual(c, (x/2.0, y/2.0, z/2.0), msg='b2Color / float')
c = c1.copy()
c *= 2.0
self.checkAlmostEqual(c, (x+x, y+y, z+z), msg='b2Color *= float')
c = b2Color(c1)
c /= 2.0
self.checkAlmostEqual(c, (x/2.0, y/2.0, z/2.0), msg='b2Color /= float')
c1 += (1.0, 1.0, 1.0)
self.checkAlmostEqual(c1, (x+1, y+1, z+1), msg='b2Color +=')
c1 -= (1.0, 1.0, 1.0)
self.checkAlmostEqual(c1, (x, y, z), msg='b2Color -=')
bytes=b2Color(1, 1, 1).bytes
self.assertEqual(bytes, [255,255,255], msg='bytes (1,1,1)=>%s'%bytes)
bytes=b2Color(0, 0, 0).bytes
self.assertEqual(bytes, [0,0,0], msg='bytes (1,1,1)=>%s'%bytes)
self.assertEqual(c1[0], x)
self.assertEqual(c1[1], y)
self.assertEqual(c1[2], z)
c1.list = (x*2, y*2, z*2)
self.checkAlmostEqual(c1, (x+x, y+y, z+z), msg='b2Color.list')
def Step(self, settings):
Framework.Step(self, settings)
renderer = self.renderer
try:
poly = b2PolygonShape(vertices=self.verts)
except AssertionError as ex:
self.Print('b2PolygonShape failed: %s' % ex)
else:
self.Print('Valid: %s' % poly.valid)
renderer.DrawPolygon([renderer.to_screen(v)
for v in self.verts], b2Color(0.9, 0.9, 0.9))
for i, v in enumerate(self.verts):
renderer.DrawPoint(renderer.to_screen(v), 2.0,
b2Color(0.9, 0.5, 0.5))
x, y = renderer.to_screen(v)
self.DrawStringAt(x + 0.05, y + 0.05, '%d' % i)
if self.auto:
self.generate()
def Step(self, settings):
Framework.Step(self, settings)
renderer = self.renderer
try:
poly = b2PolygonShape(vertices=self.verts)
except AssertionError as ex:
self.Print('b2PolygonShape failed: %s' % ex)
else:
self.Print('Valid: %s' % poly.valid)
renderer.DrawPolygon([renderer.to_screen(v)
for v in self.verts], b2Color(0.9, 0.9, 0.9))
for i, v in enumerate(self.verts):
renderer.DrawPoint(renderer.to_screen(v), 2.0,
b2Color(0.9, 0.5, 0.5))
x, y = renderer.to_screen(v)
self.DrawStringAt(x + 0.05, y + 0.05, '%d' % i)
if self.auto:
self.generate()
def DrawShape(self, shape, transform, color, selected=False):
"""
Draw any type of shape
"""
cache_hit = False
if hash(shape) in self.item_cache:
cache_hit = True
items = self.item_cache[hash(shape)]
items[0].setRotation(transform.angle * 180.0 / b2_pi)
if isinstance(shape, b2CircleShape):
radius = shape.radius
if items[0].radius == radius:
center = b2Mul(transform, shape.pos)
items[0].setPos(*center)
line = items[1]
axis = transform.R.x_axis
line.setLine(center[0], center[1],
(center[0] - radius * axis[0]),
(center[1] - radius * axis[1]))
else:
self._remove_from_cache(shape)
cache_hit = False
else:
items[0].setPos(*transform.position)
if not selected or cache_hit:
return
if selected:
color = b2Color(1, 1, 1)
temporary = True
else:
temporary = False
if isinstance(shape, b2PolygonShape):
self.DrawPolygonShape(shape, transform, color, temporary)
elif isinstance(shape, b2EdgeShape):
v1 = b2Mul(transform, shape.vertex1)
v2 = b2Mul(transform, shape.vertex2)
self.DrawSegment(v1, v2, color)
elif isinstance(shape, b2CircleShape):
self.DrawCircleShape(shape, transform, color, temporary)
elif isinstance(shape, b2LoopShape):
vertices = shape.vertices
v1 = b2Mul(transform, vertices[-1])
for v2 in vertices:
v2 = b2Mul(transform, v2)
self.DrawSegment(v1, v2, color)
v1 = v2
def DrawShape(self, shape, transform, color, selected=False):
"""
Draw any type of shape
"""
cache_hit = False
if hash(shape) in self.item_cache:
cache_hit = True
items = self.item_cache[hash(shape)]
items[0].setRotation(transform.angle * 180.0 / b2_pi)
if isinstance(shape, b2CircleShape):
radius = shape.radius
if items[0].radius == radius:
center = b2Mul(transform, shape.pos)
items[0].setPos(*center)
line = items[1]
axis = transform.R.x_axis
line.setLine(center[0], center[1],
(center[0] - radius * axis[0]),
(center[1] - radius * axis[1]))
else:
self._remove_from_cache(shape)
cache_hit = False
else:
items[0].setPos(*transform.position)
if not selected or cache_hit:
return
if selected:
color = b2Color(1, 1, 1)
temporary = True
else:
temporary = False
if isinstance(shape, b2PolygonShape):
self.DrawPolygonShape(shape, transform, color, temporary)
elif isinstance(shape, b2EdgeShape):
v1 = b2Mul(transform, shape.vertex1)
v2 = b2Mul(transform, shape.vertex2)
self.DrawSegment(v1, v2, color)
elif isinstance(shape, b2CircleShape):
self.DrawCircleShape(shape, transform, color, temporary)
elif isinstance(shape, b2LoopShape):
vertices = shape.vertices
v1 = b2Mul(transform, vertices[-1])
for v2 in vertices:
v2 = b2Mul(transform, v2)
self.DrawSegment(v1, v2, color)
v1 = v2
def Step(self, settings):
Framework.Step(self, settings)
if self.go and settings.hz > 0.0:
self.time += 1.0 / settings.hz
linear_offset = (6 * sin(2.0 * self.time), 8.0 +
4.0 * sin(1.0 * self.time))
angular_offset = 4.0 * self.time
self.joint.linearOffset = linear_offset
self.joint.angularOffset = angular_offset
renderer = self.renderer
renderer.DrawPoint(renderer.to_screen(
linear_offset), 4, b2Color(0.9, 0.9, 0.9))
def Step(self, settings):
Framework.Step(self, settings)
if self.go and settings.hz > 0.0:
self.time += 1.0 / settings.hz
linear_offset = (6 * sin(2.0 * self.time), 8.0 +
4.0 * sin(1.0 * self.time))
angular_offset = 4.0 * self.time
self.joint.linearOffset = linear_offset
self.joint.angularOffset = angular_offset
renderer = self.renderer
renderer.DrawPoint(renderer.to_screen(
linear_offset), 4, b2Color(0.9, 0.9, 0.9))
def Step(self, settings):
Framework.Step(self, settings)
if self.go and settings.hz > 0.0:
self.time += 1.0 / settings.hz
linear_offset =self.qross.position# (6 * sin(2.0 * self.time), 8.0 +
#4.0 * sin(1.0 * self.time))
angular_offset = -2.0 * self.time
center=[self.qross.position]
print(type(center))
self.joint.linearOffset = linear_offset#linear_offset
self.joint.angularOffset = angular_offset
renderer = self.renderer
renderer.DrawPoint(renderer.to_screen(
linear_offset), 4, b2Color(0.9, 0.9, 0.9))
self.viewCenter=(self.qross.position)
def _draw_joint(self, joint):
bodyA, bodyB = joint.bodyA, joint.bodyB
xf1, xf2 = bodyA.transform, bodyB.transform
x1, x2 = xf1.position, xf2.position
p1, p2 = joint.anchorA, joint.anchorB
color = b2Color(0.5, 0.8, 0.8)
x1, x2, p1, p2 = self._fix_vertices((x1 * self._ppm, x2 * self._ppm,
p1 * self._ppm, p2 * self._ppm))
if isinstance(joint, b2DistanceJoint):
cv2.line(self.screen, cvcoord(p1), cvcoord(p2), cvcolor(color), 1)
elif isinstance(joint, b2PulleyJoint):
s1, s2 = joint.groundAnchorA, joint.groundAnchorB
s1, s2 = self._fix_vertices((s1 * self._ppm, s2 * self._ppm))
cv2.line(self.screen, cvcoord(s1), cvcoord(p1), cvcolor(color), 1)
cv2.line(self.screen, cvcoord(s2), cvcoord(p2), cvcolor(color), 1)
cv2.line(self.screen, cvcoord(s1), cvcoord(s2), cvcolor(color), 1)
elif isinstance(joint, b2MouseJoint):
pass # don't draw it here
else:
cv2.line(self.screen, cvcoord(x1), cvcoord(p1), cvcolor(color), 1)
cv2.line(self.screen, cvcoord(p1), cvcoord(p2), cvcolor(color), 1)
cv2.line(self.screen, cvcoord(x2), cvcoord(p2), cvcolor(color), 1)
def DrawJoint(self, joint):
"""
Draw any type of joint
"""
bodyA, bodyB = joint.bodyA, joint.bodyB
xf1, xf2 = bodyA.transform, bodyB.transform
x1, x2 = xf1.position, xf2.position
p1, p2 = joint.anchorA, joint.anchorB
color = b2Color(0.5, 0.8, 0.8)
if isinstance(joint, b2DistanceJoint):
self.DrawSegment(p1, p2, color)
elif isinstance(joint, b2PulleyJoint):
s1, s2 = joint.groundAnchorA, joint.groundAnchorB
self.DrawSegment(s1, p1, color)
self.DrawSegment(s2, p2, color)
self.DrawSegment(s1, s2, color)
elif isinstance(joint, b2MouseJoint):
pass # don't draw it here
else:
self.DrawSegment(x1, p1, color)
self.DrawSegment(p1, p2, color)
self.DrawSegment(x2, p2, color)
def _draw_joint(self, joint):
bodyA, bodyB = joint.bodyA, joint.bodyB
xf1, xf2 = bodyA.transform, bodyB.transform
x1, x2 = xf1.position, xf2.position
p1, p2 = joint.anchorA, joint.anchorB
color = b2Color(0.5, 0.8, 0.8)
x1, x2, p1, p2 = self._fix_vertices(
(x1 * self._ppm, x2 * self._ppm, p1 * self._ppm, p2 * self._ppm))
if isinstance(joint, b2DistanceJoint):
cv2.line(self.screen, cvcoord(p1), cvcoord(p2), cvcolor(color), 1)
elif isinstance(joint, b2PulleyJoint):
s1, s2 = joint.groundAnchorA, joint.groundAnchorB
s1, s2 = self._fix_vertices((s1 * self._ppm, s2 * self._ppm))
cv2.line(self.screen, cvcoord(s1), cvcoord(p1), cvcolor(color), 1)
cv2.line(self.screen, cvcoord(s2), cvcoord(p2), cvcolor(color), 1)
cv2.line(self.screen, cvcoord(s1), cvcoord(s2), cvcolor(color), 1)
elif isinstance(joint, b2MouseJoint):
pass # don't draw it here
else:
cv2.line(self.screen, cvcoord(x1), cvcoord(p1), cvcolor(color), 1)
cv2.line(self.screen, cvcoord(p1), cvcoord(p2), cvcolor(color), 1)
cv2.line(self.screen, cvcoord(x2), cvcoord(p2), cvcolor(color), 1)
def DrawJoint(self, joint):
"""
Draw any type of joint
"""
bodyA, bodyB = joint.bodyA, joint.bodyB
xf1, xf2 = bodyA.transform, bodyB.transform
x1, x2 = xf1.position, xf2.position
p1, p2 = joint.anchorA, joint.anchorB
color = b2Color(0.5, 0.8, 0.8)
if isinstance(joint, b2DistanceJoint):
self.DrawSegment(p1, p2, color)
elif isinstance(joint, b2PulleyJoint):
s1, s2 = joint.groundAnchorA, joint.groundAnchorB
self.DrawSegment(s1, p1, color)
self.DrawSegment(s2, p2, color)
self.DrawSegment(s1, s2, color)
elif isinstance(joint, b2MouseJoint):
pass # don't draw it here
else:
self.DrawSegment(x1, p1, color)
self.DrawSegment(p1, p2, color)
self.DrawSegment(x2, p2, color)
class FrameworkBase(b2ContactListener):
"""
The base of the main testbed framework.
If you are planning on using the testbed framework and:
* Want to implement your own renderer (other than Pygame, etc.):
You should derive your class from this one to implement your own tests.
See empty.py or any of the other tests for more information.
* Do NOT want to implement your own renderer:
You should derive your class from Framework. The renderer chosen in
fwSettings (see settings.py) or on the command line will automatically
be used for your test.
"""
name = "None"
description = None
TEXTLINE_START = 30
colors = {
'mouse_point': b2Color(0, 1, 0),
'joint_line': b2Color(0.8, 0.8, 0.8),
'contact_add': b2Color(0.3, 0.95, 0.3),
'contact_persist': b2Color(0.3, 0.3, 0.95),
'contact_normal': b2Color(0.4, 0.9, 0.4),
}
def __reset(self):
""" Reset all of the variables to their starting values.
Not to be called except at initialization."""
# Box2D-related
self.points = []
self.world = None
self.mouseJoint = None
# self.settings = fwSettings
self.mouseWorld = None
self.using_contacts = False
self.stepCount = 0
# Box2D-callbacks
# TODO: add raycast here or not?
self.raycastListener = None
self.destructionListener = None
self.renderer = None
def __init__(self):
super(FrameworkBase, self).__init__()
self.__reset()
# Box2D Initialization
self.world = b2World(gravity=(0, 0), doSleep=True)
self.destructionListener = fwDestructionListener(test=self)
self.raycastListener = RayCastClosestCallback()
self.world.destructionListener = self.destructionListener
self.world.contactListener = self
self.t_steps, self.t_draws = [], []
def __del__(self):
pass
def Step(self, settings):
"""
The main physics step.
Takes care of physics drawing (callbacks are executed after the world.Step() )
and drawing additional information.
"""
self.stepCount += 1
# Don't do anything if the setting's Hz are <= 0
if settings.hz > 0.0:
timeStep = 1.0 / settings.hz
else:
timeStep = 0.0
renderer = self.renderer
# If paused, display so
if settings.pause:
if settings.singleStep:
settings.singleStep = False
else:
timeStep = 0.0
self.Print("****PAUSED****", (200, 0, 0))
# Set the flags based on what the settings show
if renderer:
# convertVertices is only applicable when using b2DrawExtended. It
# indicates that the C code should transform box2d coords to screen
# coordinates.
is_extended = isinstance(renderer, b2DrawExtended)
renderer.flags = dict(
drawShapes=settings.drawShapes,
drawJoints=settings.drawJoints,
drawAABBs=settings.drawAABBs,
drawPairs=settings.drawPairs,
drawCOMs=settings.drawCOMs,
convertVertices=is_extended,
)
# Set the other settings that aren't contained in the flags
self.world.warmStarting = settings.enableWarmStarting
self.world.continuousPhysics = settings.enableContinuous
self.world.subStepping = settings.enableSubStepping
# Reset the collision points
self.points = []
# Tell Box2D to step
t_step = time()
self.world.Step(timeStep, settings.velocityIterations,
settings.positionIterations)
self.world.ClearForces()
t_step = time() - t_step
# Update the debug draw settings so that the vertices will be properly
# converted to screen coordinates
t_draw = time()
if renderer is not None:
renderer.StartDraw()
# self.world.DrawDebugData()
# Take care of additional drawing (fps, mouse joint, slingshot bomb,
# contact points)
if renderer:
# If there's a mouse joint, draw the connection between the object
# and the current pointer position.
if self.mouseJoint:
p1 = renderer.to_screen(self.mouseJoint.anchorB)
p2 = renderer.to_screen(self.mouseJoint.target)
renderer.DrawPoint(p1, settings.pointSize,
self.colors['mouse_point'])
renderer.DrawPoint(p2, settings.pointSize,
self.colors['mouse_point'])
renderer.DrawSegment(p1, p2, self.colors['joint_line'])
# Draw each of the contact points in different colors.
if self.settings.drawContactPoints:
for point in self.points:
if point['state'] == b2_addState:
renderer.DrawPoint(
renderer.to_screen(point['position']),
settings.pointSize, self.colors['contact_add'])
elif point['state'] == b2_persistState:
renderer.DrawPoint(
renderer.to_screen(point['position']),
settings.pointSize, self.colors['contact_persist'])
if settings.drawContactNormals:
for point in self.points:
p1 = renderer.to_screen(point['position'])
p2 = renderer.axisScale * point['normal'] + p1
renderer.DrawSegment(p1, p2, self.colors['contact_normal'])
renderer.EndDraw()
t_draw = time() - t_draw
t_draw = max(b2_epsilon, t_draw)
t_step = max(b2_epsilon, t_step)
try:
self.t_draws.append(1.0 / t_draw)
except:
pass
else:
if len(self.t_draws) > 2:
self.t_draws.pop(0)
try:
self.t_steps.append(1.0 / t_step)
except:
pass
else:
if len(self.t_steps) > 2:
self.t_steps.pop(0)
if settings.drawFPS:
self.Print("Combined FPS %d" % self.fps)
if settings.drawStats:
self.Print("bodies=%d contacts=%d joints=%d proxies=%d" %
(self.world.bodyCount, self.world.contactCount,
self.world.jointCount, self.world.proxyCount))
self.Print("hz %d vel/pos iterations %d/%d" %
(settings.hz, settings.velocityIterations,
settings.positionIterations))
if self.t_draws and self.t_steps:
self.Print(
"Potential draw rate: %.2f fps Step rate: %.2f Hz"
"" % (sum(self.t_draws) / len(self.t_draws),
sum(self.t_steps) / len(self.t_steps)))
def ShiftMouseDown(self, p):
"""
Indicates that there was a left click at point p (world coordinates)
with the left shift key being held down.
"""
pass
def MouseDown(self, p):
"""
Indicates that there was a left click at point p (world coordinates)
"""
if self.mouseJoint is not None:
return
self.env.MouseDown(p)
def MouseUp(self, p):
"""
Left mouse button up.
"""
pass
def MouseMove(self, p):
"""
Mouse moved to point p, in world coordinates.
"""
self.env.MouseMove(p)
# self.mouseWorld = p
# if self.mouseJoint:
# self.mouseJoint.target = p
pass
def SimulationLoop(self):
"""
The main simulation loop. Don't override this, override Step instead.
"""
self.Step(self.settings)
def ConvertScreenToWorld(self, x, y):
"""
Return a b2Vec2 in world coordinates of the passed in screen
coordinates x, y
NOTE: Renderer subclasses must implement this
"""
raise NotImplementedError()
def DrawStringAt(self, x, y, str, color=(229, 153, 153, 255)):
"""
Draw some text, str, at screen coordinates (x, y).
NOTE: Renderer subclasses must implement this
"""
raise NotImplementedError()
def Print(self, str, color=(229, 153, 153, 255)):
"""
Draw some text at the top status lines
and advance to the next line.
NOTE: Renderer subclasses must implement this
"""
raise NotImplementedError()
def PreSolve(self, contact, old_manifold):
"""
This is a critical function when there are many contacts in the world.
It should be optimized as much as possible.
"""
if not (self.settings.drawContactPoints
or self.settings.drawContactNormals or self.using_contacts):
return
elif len(self.points) > self.settings.maxContactPoints:
return
manifold = contact.manifold
if manifold.pointCount == 0:
return
state1, state2 = b2GetPointStates(old_manifold, manifold)
if not state2:
return
worldManifold = contact.worldManifold
# TODO: find some way to speed all of this up.
self.points.extend([
dict(
fixtureA=contact.fixtureA,
fixtureB=contact.fixtureB,
position=worldManifold.points[i],
normal=worldManifold.normal.copy(),
state=state2[i],
) for i, point in enumerate(state2)
])
# These can/should be implemented in the test subclass: (Step() also if necessary)
# See empty.py (Box2D) for a simple example.
def BeginContact(self, contact):
# print("hit")
self.env.BeginContact(contact.fixtureA.body.userData,
contact.fixtureB.body.userData)
pass
def EndContact(self, contact):
pass
def PostSolve(self, contact, impulse):
pass
def FixtureDestroyed(self, fixture):
"""
Callback indicating 'fixture' has been destroyed.
"""
pass
def JointDestroyed(self, joint):
"""
Callback indicating 'joint' has been destroyed.
"""
pass
def Keyboard(self, key):
"""
Callback indicating 'key' has been pressed down.
"""
pass
def KeyboardUp(self, key):
"""
Callback indicating 'key' has been released.
"""
pass
class FrameworkBase(b2ContactListener):
"""
The base of the main testbed framework.
If you are planning on using the testbed framework and:
* Want to implement your own renderer (other than Pygame, etc.):
You should derive your class from this one to implement your own tests.
See empty.py or any of the other tests for more information.
* Do NOT want to implement your own renderer:
You should derive your class from Framework. The renderer chosen in
fwSettings (see settings.py) or on the command line will automatically
be used for your test.
"""
name = "None"
description = None
TEXTLINE_START = 30
colors = {
'mouse_point': b2Color(0, 1, 0),
'bomb_center': b2Color(0, 0, 1.0),
'bomb_line': b2Color(0, 1.0, 1.0),
'joint_line': b2Color(0.8, 0.8, 0.8),
'contact_add': b2Color(0.3, 0.95, 0.3),
'contact_persist': b2Color(0.3, 0.3, 0.95),
'contact_normal': b2Color(0.4, 0.9, 0.4),
}
def __reset(self):
""" Reset all of the variables to their starting values.
Not to be called except at initialization."""
# Box2D-related
self.points = []
self.world = None
self.bomb = None
self.mouseJoint = None
self.settings = fwSettings
self.bombSpawning = False
self.bombSpawnPoint = None
self.mouseWorld = None
self.using_contacts = False
self.stepCount = 0
# Box2D-callbacks
self.destructionListener = None
self.renderer = None
def __init__(self):
super(FrameworkBase, self).__init__()
self.__reset()
# Box2D Initialization
self.world = b2World(gravity=(0, -10), doSleep=True)
self.destructionListener = fwDestructionListener(test=self)
self.world.destructionListener = self.destructionListener
self.world.contactListener = self
self.t_steps, self.t_draws = [], []
def __del__(self):
pass
def Step(self, settings):
"""
The main physics step.
Takes care of physics drawing (callbacks are executed after the world.Step() )
and drawing additional information.
"""
self.stepCount += 1
# Don't do anything if the setting's Hz are <= 0
if settings.hz > 0.0:
timeStep = 1.0 / settings.hz
else:
timeStep = 0.0
renderer = self.renderer
# If paused, display so
if settings.pause:
if settings.singleStep:
settings.singleStep = False
else:
timeStep = 0.0
self.Print("****PAUSED****", (200, 0, 0))
# Set the flags based on what the settings show
if renderer:
# convertVertices is only applicable when using b2DrawExtended. It
# indicates that the C code should transform box2d coords to screen
# coordinates.
is_extended = isinstance(renderer, b2DrawExtended)
renderer.flags = dict(
drawShapes=settings.drawShapes,
drawJoints=settings.drawJoints,
drawAABBs=settings.drawAABBs,
drawPairs=settings.drawPairs,
drawCOMs=settings.drawCOMs,
convertVertices=is_extended,
)
# Set the other settings that aren't contained in the flags
self.world.warmStarting = settings.enableWarmStarting
self.world.continuousPhysics = settings.enableContinuous
self.world.subStepping = settings.enableSubStepping
# Set the thresholds
self.world.velocityThreshold = settings.velocityThreshold
self.world.positionThreshold = settings.positionThreshold
# Reset the collision points
self.points = []
# Tell Box2D to step
t_step = time()
self.world.Step(timeStep, settings.velocityIterations,
settings.positionIterations)
self.world.ClearForces()
t_step = time() - t_step
# Update the debug draw settings so that the vertices will be properly
# converted to screen coordinates
t_draw = time()
if renderer is not None:
renderer.StartDraw()
self.world.DrawDebugData()
# If the bomb is frozen, get rid of it.
if self.bomb and not self.bomb.awake:
self.world.DestroyBody(self.bomb)
self.bomb = None
# Take care of additional drawing (fps, mouse joint, slingshot bomb,
# contact points)
if renderer:
# If there's a mouse joint, draw the connection between the object
# and the current pointer position.
if self.mouseJoint:
p1 = renderer.to_screen(self.mouseJoint.anchorB)
p2 = renderer.to_screen(self.mouseJoint.target)
renderer.DrawPoint(p1, settings.pointSize,
self.colors['mouse_point'])
renderer.DrawPoint(p2, settings.pointSize,
self.colors['mouse_point'])
renderer.DrawSegment(p1, p2, self.colors['joint_line'])
# Draw the slingshot bomb
if self.bombSpawning:
renderer.DrawPoint(renderer.to_screen(self.bombSpawnPoint),
settings.pointSize,
self.colors['bomb_center'])
renderer.DrawSegment(renderer.to_screen(self.bombSpawnPoint),
renderer.to_screen(self.mouseWorld),
self.colors['bomb_line'])
# Draw each of the contact points in different colors.
if self.settings.drawContactPoints:
for point in self.points:
if point['state'] == b2_addState:
renderer.DrawPoint(
renderer.to_screen(point['position']),
settings.pointSize, self.colors['contact_add'])
elif point['state'] == b2_persistState:
renderer.DrawPoint(
renderer.to_screen(point['position']),
settings.pointSize, self.colors['contact_persist'])
if settings.drawContactNormals:
for point in self.points:
p1 = renderer.to_screen(point['position'])
p2 = renderer.axisScale * point['normal'] + p1
renderer.DrawSegment(p1, p2, self.colors['contact_normal'])
renderer.EndDraw()
t_draw = time() - t_draw
t_draw = max(b2_epsilon, t_draw)
t_step = max(b2_epsilon, t_step)
try:
self.t_draws.append(1.0 / t_draw)
except:
pass
else:
if len(self.t_draws) > 2:
self.t_draws.pop(0)
try:
self.t_steps.append(1.0 / t_step)
except:
pass
else:
if len(self.t_steps) > 2:
self.t_steps.pop(0)
if settings.drawFPS:
self.Print("Combined FPS %d" % self.fps)
if settings.drawStats:
self.Print("bodies=%d contacts=%d joints=%d proxies=%d" %
(self.world.bodyCount, self.world.contactCount,
self.world.jointCount, self.world.proxyCount))
self.Print("hz %d vel/pos iterations %d/%d" %
(settings.hz, settings.velocityIterations,
settings.positionIterations))
if self.t_draws and self.t_steps:
self.Print(
"Potential draw rate: %.2f fps Step rate: %.2f Hz"
"" % (sum(self.t_draws) / len(self.t_draws),
sum(self.t_steps) / len(self.t_steps)))
def ShiftMouseDown(self, p):
"""
Indicates that there was a left click at point p (world coordinates)
with the left shift key being held down.
"""
self.mouseWorld = p
if not self.mouseJoint:
self.SpawnBomb(p)
def MouseDown(self, p):
"""
Indicates that there was a left click at point p (world coordinates)
"""
if self.mouseJoint is not None:
return
# Create a mouse joint on the selected body (assuming it's dynamic)
# Make a small box.
aabb = b2AABB(lowerBound=p - (0.001, 0.001),
upperBound=p + (0.001, 0.001))
# Query the world for overlapping shapes.
query = fwQueryCallback(p)
self.world.QueryAABB(query, aabb)
if query.fixture:
body = query.fixture.body
# A body was selected, create the mouse joint
self.mouseJoint = self.world.CreateMouseJoint(
bodyA=self.groundbody,
bodyB=body,
target=p,
maxForce=1000.0 * body.mass)
body.awake = True
def MouseUp(self, p):
"""
Left mouse button up.
"""
if self.mouseJoint:
self.world.DestroyJoint(self.mouseJoint)
self.mouseJoint = None
if self.bombSpawning:
self.CompleteBombSpawn(p)
def MouseMove(self, p):
"""
Mouse moved to point p, in world coordinates.
"""
self.mouseWorld = p
if self.mouseJoint:
self.mouseJoint.target = p
def SpawnBomb(self, worldPt):
"""
Begins the slingshot bomb by recording the initial position.
Once the user drags the mouse and releases it, then
CompleteBombSpawn will be called and the actual bomb will be
released.
"""
self.bombSpawnPoint = worldPt.copy()
self.bombSpawning = True
def CompleteBombSpawn(self, p):
"""
Create the slingshot bomb based on the two points
(from the worldPt passed to SpawnBomb to p passed in here)
"""
if not self.bombSpawning:
return
multiplier = 30.0
vel = self.bombSpawnPoint - p
vel *= multiplier
self.LaunchBomb(self.bombSpawnPoint, vel)
self.bombSpawning = False
def LaunchBomb(self, position, velocity):
"""
A bomb is a simple circle which has the specified position and velocity.
position and velocity must be b2Vec2's.
"""
if self.bomb:
self.world.DestroyBody(self.bomb)
self.bomb = None
self.bomb = self.world.CreateDynamicBody(
allowSleep=True,
position=position,
linearVelocity=velocity,
fixtures=b2FixtureDef(shape=b2CircleShape(radius=0.3),
density=20,
restitution=0.1))
def LaunchRandomBomb(self):
"""
Create a new bomb and launch it at the testbed.
"""
p = b2Vec2(b2Random(-15.0, 15.0), 30.0)
v = -5.0 * p
self.LaunchBomb(p, v)
def SimulationLoop(self):
"""
The main simulation loop. Don't override this, override Step instead.
"""
# Reset the text line to start the text from the top
self.textLine = self.TEXTLINE_START
# Draw the name of the test running
self.Print(self.name, (127, 127, 255))
if self.description:
# Draw the name of the test running
for s in self.description.split('\n'):
self.Print(s, (127, 255, 127))
# Do the main physics step
self.Step(self.settings)
def ConvertScreenToWorld(self, x, y):
"""
Return a b2Vec2 in world coordinates of the passed in screen
coordinates x, y
NOTE: Renderer subclasses must implement this
"""
raise NotImplementedError()
def DrawStringAt(self, x, y, str, color=(229, 153, 153, 255)):
"""
Draw some text, str, at screen coordinates (x, y).
NOTE: Renderer subclasses must implement this
"""
raise NotImplementedError()
def Print(self, str, color=(229, 153, 153, 255)):
"""
Draw some text at the top status lines
and advance to the next line.
NOTE: Renderer subclasses must implement this
"""
raise NotImplementedError()
def PreSolve(self, contact, old_manifold):
"""
This is a critical function when there are many contacts in the world.
It should be optimized as much as possible.
"""
if not (self.settings.drawContactPoints
or self.settings.drawContactNormals or self.using_contacts):
return
elif len(self.points) > self.settings.maxContactPoints:
return
manifold = contact.manifold
if manifold.pointCount == 0:
return
state1, state2 = b2GetPointStates(old_manifold, manifold)
if not state2:
return
worldManifold = contact.worldManifold
# TODO: find some way to speed all of this up.
self.points.extend([
dict(
fixtureA=contact.fixtureA,
fixtureB=contact.fixtureB,
position=worldManifold.points[i],
normal=worldManifold.normal.copy(),
state=state2[i],
) for i, point in enumerate(state2)
])
# These can/should be implemented in the test subclass: (Step() also if necessary)
# See empty.py for a simple example.
def BeginContact(self, contact):
pass
def EndContact(self, contact):
pass
def PostSolve(self, contact, impulse):
pass
def FixtureDestroyed(self, fixture):
"""
Callback indicating 'fixture' has been destroyed.
"""
pass
def JointDestroyed(self, joint):
"""
Callback indicating 'joint' has been destroyed.
"""
pass
def Keyboard(self, key):
"""
Callback indicating 'key' has been pressed down.
"""
pass
def KeyboardUp(self, key):
"""
Callback indicating 'key' has been released.
"""
pass
def step(self, actions=None):
# print(settings)
if self.done: # "Episode is finished."
self.quit() # TODO: is quit() necessary?
if actions == None:
actions = {}
for agent in self.agents:
if not agent.alive:
continue
act = agent.actor(self.obs[agent.id])
actions[agent.id] = act
assert self.action_space.contains(actions)
# Agent actions
for agent in self.agents:
if not agent.alive:
continue
# 0,1,2 = CW, NOOP, CCW
# Rotation
agent.body.angle = (
agent.body.angle +
(actions[agent.id][2] - 1) * agent.rotation_speed *
(1 / self.settings.hz))
if np.abs(agent.body.angle) > np.pi:
agent.body.angle -= np.sign(agent.body.angle) * (2 * np.pi)
# Movement
angle = agent.body.angle
c = 1 / np.sqrt(2) if ((actions[agent.id][0] != 1) and
(actions[agent.id][1] != 1)) else 1
x_force = (np.cos(angle) *
(actions[agent.id][0] - 1) + np.cos(angle + np.pi / 2) *
(actions[agent.id][1] - 1)
) * c * agent.force #* (1/self.settings.hz)
y_force = (np.sin(angle) *
(actions[agent.id][0] - 1) + np.sin(angle + np.pi / 2) *
(actions[agent.id][1] - 1)
) * c * agent.force #* (1/self.settings.hz)
agent.body.ApplyForce(force=(x_force, y_force),
point=agent.body.position,
wake=True)
# Attack
if agent.cooldown_atk <= 0:
if actions[agent.id][3]:
point1 = agent.body.position
d = (agent.melee_range * np.cos(agent.body.angle),
agent.melee_range * np.sin(agent.body.angle))
point2 = point1 + d
self.framework.world.RayCast(
self.framework.raycastListener, point1, point2)
if self.framework.renderer:
self.framework.renderer.DrawSegment(
point1, point2, b2Color(0.8, 0.8, 0.8))
agent.cooldown_atk = self.cooldown_atk
agent.cooldown_mov_penalty = self.cooldown_mov_penalty
if self.framework.raycastListener.hit:
self.framework.raycastListener.fixture.body.userData.health -= agent.melee_dmg
else:
agent.cooldown_atk -= (1 / self.settings.hz)
# Agent deaths
for agent in self.agents:
if not agent.alive:
continue
if agent.health <= 0:
agent.alive = False
agent.body.active = False
self.n_alive[agent.team] -= 1
# We recreate action and obs space when n of agents change
self.create_space()
# super(World, self).Step(self.settings)
self.framework.Step(self.settings)
self.obs = self.get_obs()
rewards, done = None, None
self.time_passed += (1 / self.settings.hz)
alive_teams = [i for i, e in enumerate(self.n_alive) if e != 0]
if (self.time_passed > self.time_limit):
self.done = True
# print("Time is up")
if len(alive_teams) == 1:
self.done = True
self.winner = alive_teams[0]
# print("winner: team %s" % alive_teams[0])
if len(alive_teams) == 0:
self.done = True
# print("Draw")
return self.obs
from Box2D import b2Color TARGET_FPS = 60 TIME_STEP = 0.01 / TARGET_FPS DRAW_GRAPHICS = True #TARGET_FPS = 60 ZOOM = 5.0 FIELD_H = (174) # (1) Height of the screen pygame with the Box2D FIELD_W = (134) # (2) Width of the screen pygame with the Box2D CORRECTION_FATOR_METER_TO_CM = 10**2 # <==== Very important: 1 m = 1*10^2 cm MAX_ROBOTS_ALLIES = 5 # CENTER_AXIS_X = -0 # CENTER_AXIS_Y = 0 # Proprerties of bodies MASS_BALL = 0.046 # Kg MASS_ROBOT = 5.100 # Kg (adjust later) # --- Colors --- BLUE = b2Color(0,0,1) YELLOW = b2Color(1,1,0) WHITE = b2Color(1,1,1)
class Raycast(Framework):
name = "Raycast"
description = "Press 1-5 to drop stuff, d to delete, m to switch callback modes"
p1_color = b2Color(0.4, 0.9, 0.4)
s1_color = b2Color(0.8, 0.8, 0.8)
s2_color = b2Color(0.9, 0.9, 0.4)
def __init__(self):
super(Raycast, self).__init__()
self.world.gravity = (0, 0)
# The ground
ground = self.world.CreateBody(shapes=b2EdgeShape(vertices=[(-40,
0), (40,
0)]))
# The various shapes
w = 1.0
b = w / (2.0 + sqrt(2.0))
s = sqrt(2.0) * b
self.shapes = [
b2PolygonShape(vertices=[(-0.5, 0), (0.5, 0), (0, 1.5)]),
b2PolygonShape(vertices=[(-0.1, 0), (0.1, 0), (0, 1.5)]),
b2PolygonShape(
vertices=[(0.5 * s,
0), (0.5 * w,
b), (0.5 * w,
b + s), (0.5 * s,
w), (-0.5 * s,
w), (-0.5 * w, b +
s), (-0.5 * w,
b), (-0.5 * s,
0.0)]),
b2PolygonShape(box=(0.5, 0.5)),
b2CircleShape(radius=0.5),
]
self.angle = 0
self.callbacks = [
RayCastClosestCallback, RayCastAnyCallback, RayCastMultipleCallback
]
self.callback_class = self.callbacks[0]
def CreateShape(self, shapeindex):
try:
shape = self.shapes[shapeindex]
except IndexError:
return
pos = (10.0 * (2.0 * random() - 1.0), 10.0 * (2.0 * random()))
defn = b2BodyDef(
type=b2_dynamicBody,
fixtures=b2FixtureDef(shape=shape, friction=0.3),
position=pos,
angle=(b2_pi * (2.0 * random() - 1.0)),
)
if isinstance(shape, b2CircleShape):
defn.angularDamping = 0.02
self.world.CreateBody(defn)
def DestroyBody(self):
for body in self.world.bodies:
if not self.world.locked:
self.world.DestroyBody(body)
break
def Keyboard(self, key):
if key in (Keys.K_1, Keys.K_2, Keys.K_3, Keys.K_4, Keys.K_5):
self.CreateShape(key - Keys.K_1)
elif key == Keys.K_d:
self.DestroyBody()
elif key == Keys.K_m:
idx = ((self.callbacks.index(self.callback_class) + 1) %
len(self.callbacks))
self.callback_class = self.callbacks[idx]
def Step(self, settings):
super(Raycast, self).Step(settings)
def draw_hit(cb_point, cb_normal):
cb_point = self.renderer.to_screen(cb_point)
head = b2Vec2(cb_point) + 0.5 * cb_normal
cb_normal = self.renderer.to_screen(cb_normal)
self.renderer.DrawPoint(cb_point, 5.0, self.p1_color)
self.renderer.DrawSegment(point1, cb_point, self.s1_color)
self.renderer.DrawSegment(cb_point, head, self.s2_color)
# Set up the raycast line
length = 11
point1 = b2Vec2(0, 10)
d = (length * cos(self.angle), length * sin(self.angle))
point2 = point1 + d
callback = self.callback_class()
self.world.RayCast(callback, point1, point2)
# The callback has been called by this point, and if a fixture was hit it will have been
# set to callback.fixture.
point1 = self.renderer.to_screen(point1)
point2 = self.renderer.to_screen(point2)
if callback.hit:
if hasattr(callback, 'points'):
for point, normal in zip(callback.points, callback.normals):
draw_hit(point, normal)
else:
draw_hit(callback.point, callback.normal)
else:
self.renderer.DrawSegment(point1, point2, self.s1_color)
self.Print("Callback: %s" % callback)
if callback.hit:
self.Print("Hit")
if not settings.pause or settings.singleStep:
self.angle += 0.25 * b2_pi / 180
class EdgeShapes(Framework):
name = "Edge Shapes"
description = "Press 1-5 to drop stuff, and d to delete"
p1_color = b2Color(0.4, 0.9, 0.4)
s1_color = b2Color(0.8, 0.8, 0.8)
s2_color = b2Color(0.9, 0.9, 0.4)
def __init__(self):
super(EdgeShapes, self).__init__()
self.ground = self.world.CreateStaticBody(shapes=[
b2EdgeShape(vertices=v)
for v in get_sinusoid_vertices(-20.0, VERTEX_COUNT)
])
self.shapes = [
b2PolygonShape(vertices=[(-0.5, 0), (0.5, 0), (0, 1.5)]),
b2PolygonShape(vertices=[(-0.1, 0), (0.1, 0), (0, 1.5)]),
b2PolygonShape(vertices=get_octagon_vertices(1.0)),
b2PolygonShape(box=(0.5, 0.5)),
b2CircleShape(radius=0.5),
]
self.angle = 0
self.callback = RayCastCallback()
@property
def bodies(self):
return [body for body in self.world.bodies if body != self.ground]
def CreateShape(self, shapeindex):
try:
shape = self.shapes[shapeindex]
except IndexError:
return
pos = (10.0 * (2.0 * random() - 1.0), 10.0 * (2.0 * random() + 1.0))
defn = b2BodyDef(
type=b2_dynamicBody,
fixtures=b2FixtureDef(shape=shape, friction=0.3),
position=pos,
angle=(b2_pi * (2.0 * random() - 1.0)),
)
if isinstance(shape, b2CircleShape):
defn.angularDamping = 0.02
self.world.CreateBody(defn)
def DestroyBody(self):
if not self.world.locked:
for body in self.bodies:
self.world.DestroyBody(body)
break
def Keyboard(self, key):
if key in (Keys.K_1, Keys.K_2, Keys.K_3, Keys.K_4, Keys.K_5):
self.CreateShape(key - Keys.K_1)
elif key == Keys.K_d:
self.DestroyBody()
def Step(self, settings):
super(EdgeShapes, self).Step(settings)
# Set up the raycast line
length = 25.0
point1 = b2Vec2(0, 10)
d = (length * cos(self.angle), length * sin(self.angle))
point2 = point1 + d
callback = self.callback
callback.fixture = None
self.world.RayCast(callback, point1, point2)
# The callback has been called by this point, and if a fixture was hit it will have been
# set to callback.fixture.
point1 = self.renderer.to_screen(point1)
point2 = self.renderer.to_screen(point2)
if callback.fixture:
cb_point = self.renderer.to_screen(callback.point)
self.renderer.DrawPoint(cb_point, 5.0, self.p1_color)
self.renderer.DrawSegment(point1, cb_point, self.s1_color)
head = b2Vec2(cb_point) + 0.5 * callback.normal
self.renderer.DrawSegment(cb_point, head, self.s2_color)
else:
self.renderer.DrawSegment(point1, point2, self.s1_color)
if not settings.pause or settings.singleStep:
self.angle += 0.25 * b2_pi / 180
class Distance(Framework):
name = "Distance"
description = ("Use WASD to move and QE to rotate the small rectangle.\n"
"The distance between the marked points is shown.")
point_a_color = b2Color(1, 0, 0)
point_b_color = b2Color(1, 1, 0)
poly_color = b2Color(0.9, 0.9, 0.9)
def __init__(self):
super(Distance, self).__init__()
# Transform A -- a simple translation/offset of (0,-0.2)
self.transformA = b2Transform()
self.transformA.SetIdentity()
self.transformA.position = (0, -0.2)
# Transform B -- a translation and a rotation
self.transformB = b2Transform()
self.positionB = b2Vec2(12.017401, 0.13678508)
self.angleB = -0.0109265
self.transformB.Set(self.positionB, self.angleB)
# The two shapes, transformed by the respective transform[A,B]
self.polygonA = b2PolygonShape(box=(10, 0.2))
self.polygonB = b2PolygonShape(box=(2, 0.1))
def Step(self, settings):
super(Distance, self).Step(settings)
# Calculate the distance between the two shapes with the specified
# transforms
dist_result = b2Distance(shapeA=self.polygonA,
shapeB=self.polygonB,
transformA=self.transformA,
transformB=self.transformB)
pointA, pointB, distance, iterations = dist_result
self.Print('Distance = %g' % distance)
self.Print('Iterations = %d' % iterations)
# Manually transform the vertices and draw the shapes
for shape, transform in [(self.polygonA, self.transformA),
(self.polygonB, self.transformB)]:
new_verts = [
self.renderer.to_screen(transform * v) for v in shape.vertices
]
self.renderer.DrawPolygon(new_verts, self.poly_color)
self.renderer.DrawPoint(self.renderer.to_screen(pointA), 4,
self.point_a_color)
self.renderer.DrawPoint(self.renderer.to_screen(pointB), 4,
self.point_b_color)
def Keyboard(self, key):
if key == Keys.K_a:
self.positionB -= (0.1, 0)
elif key == Keys.K_d:
self.positionB += (0.1, 0)
elif key == Keys.K_w:
self.positionB += (0, 0.1)
elif key == Keys.K_s:
self.positionB -= (0, 0.1)
elif key == Keys.K_q:
self.angleB += 0.1 * b2_pi
elif key == Keys.K_e:
self.angleB -= 0.1 * b2_pi
self.transformB.Set(self.positionB, self.angleB)
def ManualDraw(self):
"""
This implements code normally present in the C++ version, which calls
the callbacks that you see in this class (DrawSegment, DrawSolidCircle,
etc.).
This is implemented in Python as an example of how to do it, and also a
test.
"""
colors = {
'active': b2Color(0.5, 0.5, 0.3),
'static': b2Color(0.5, 0.9, 0.5),
'kinematic': b2Color(0.5, 0.5, 0.9),
'asleep': b2Color(0.6, 0.6, 0.6),
'default': b2Color(0.9, 0.7, 0.7),
}
settings = self.test.settings
world = self.test.world
if self.test.selected_shapebody:
sel_shape, sel_body = self.test.selected_shapebody
else:
sel_shape = None
if settings.drawShapes:
for body in world.bodies:
transform = body.transform
for fixture in body.fixtures:
shape = fixture.shape
if not body.active:
color = colors['active']
elif body.type == b2_staticBody:
color = colors['static']
elif body.type == b2_kinematicBody:
color = colors['kinematic']
elif not body.awake:
color = colors['asleep']
else:
color = colors['default']
self.DrawShape(fixture.shape, transform, color,
(sel_shape == shape))
if settings.drawJoints:
for joint in world.joints:
self.DrawJoint(joint)
# if settings.drawPairs
# pass
if settings.drawAABBs:
color = b2Color(0.9, 0.3, 0.9)
# cm = world.contactManager
for body in world.bodies:
if not body.active:
continue
transform = body.transform
for fixture in body.fixtures:
shape = fixture.shape
for childIndex in range(shape.childCount):
self.DrawAABB(shape.getAABB(transform, childIndex),
color)
def Step(self, settings):
super(TimeOfImpact, self).Step(settings)
# b2Sweep describes the motion of a body/shape for TOI computation.
# Shapes are defined with respect to the body origin, which may no
# coincide with the center of mass. However, to support dynamics we
# must interpolate the center of mass position.
sweepA = b2Sweep(c0=(0, 0), c=(0, 0),
a=0, a0=0,
localCenter=(0, 0))
# The parameters of the sweep are defined as follows:
# localCenter - local center of mass position
# c0, c - center world positions
# a0, a - world angles
# t0 - time interval = [t0,1], where t0 is in [0,1]
sweepB = b2Sweep(c0=(-0.20382018, 2.1368704),
a0=-3.1664171,
c=(-0.26699525, 2.3552670),
a=-3.3926492,
localCenter=(0, 0))
type_, time_of_impact = b2TimeOfImpact(shapeA=self.shapeA,
shapeB=self.shapeB,
sweepA=sweepA, sweepB=sweepB,
tMax=1.0)
# Alternative pybox2d syntax (no kwargs):
# type_, t = b2TimeOfImpact(self.shapeA, self.shapeB, sweepA, sweepB, 1.0)
#
# And even uglier:
# input=b2TOIInput(proxyA=b2DistanceProxy(shape=self.shapeA), proxyB=b2DistanceProxy(shape=self.shapeB), sweepA=sweepA, sweepB=sweepB, tMax=1.0)
# type_, t = b2TimeOfImpact(input)
self.Print("TOI = %g" % time_of_impact)
self.Print("max toi iters = %d, max root iters = %d" %
(b2Globals.b2_toiMaxIters, b2Globals.b2_toiMaxRootIters))
# Draw the shapes at their current position (t=0)
# shapeA (the vertical polygon)
transform = sweepA.GetTransform(0)
self.renderer.DrawPolygon([self.renderer.to_screen(transform * v)
for v in self.shapeA.vertices],
b2Color(0.9, 0.9, 0.9))
# shapeB (the horizontal polygon)
transform = sweepB.GetTransform(0)
self.renderer.DrawPolygon([self.renderer.to_screen(transform * v)
for v in self.shapeB.vertices],
b2Color(0.5, 0.9, 0.5))
# localPoint=(2, -0.1)
# rB = transform * localPoint - sweepB.c0
# wB = sweepB.a - sweepB.a0
# vB = sweepB.c - sweepB.c0
# v = vB + b2Cross(wB, rB)
# Now, draw shapeB in a different color when they would collide (i.e.,
# at t=time of impact) This shows that the polygon would rotate upon
# collision
transform = sweepB.GetTransform(time_of_impact)
self.renderer.DrawPolygon([self.renderer.to_screen(transform * v)
for v in self.shapeB.vertices],
b2Color(0.5, 0.7, 0.9))
# And finally, draw shapeB at t=1.0, where it would be if it did not
# collide with shapeA In this case, time_of_impact = 1.0, so these
# become the same polygon.
transform = sweepB.GetTransform(1.0)
self.renderer.DrawPolygon([self.renderer.to_screen(transform * v)
for v in self.shapeB.vertices],
b2Color(0.9, 0.5, 0.5))
def ManualDraw(self):
"""
This implements code normally present in the C++ version, which calls
the callbacks that you see in this class (DrawSegment, DrawSolidCircle,
etc.).
This is implemented in Python as an example of how to do it, and also a
test.
"""
colors = {
'active': b2Color(0.5, 0.5, 0.3),
'static': b2Color(0.5, 0.9, 0.5),
'kinematic': b2Color(0.5, 0.5, 0.9),
'asleep': b2Color(0.6, 0.6, 0.6),
'default': b2Color(0.9, 0.7, 0.7),
}
settings = self.test.settings
world = self.test.world
if self.test.selected_shapebody:
sel_shape, sel_body = self.test.selected_shapebody
else:
sel_shape = None
if settings.drawShapes:
for body in world.bodies:
transform = body.transform
for fixture in body.fixtures:
shape = fixture.shape
if not body.active:
color = colors['active']
elif body.type == b2_staticBody:
color = colors['static']
elif body.type == b2_kinematicBody:
color = colors['kinematic']
elif not body.awake:
color = colors['asleep']
else:
color = colors['default']
self.DrawShape(fixture.shape, transform,
color, (sel_shape == shape))
if settings.drawJoints:
for joint in world.joints:
self.DrawJoint(joint)
# if settings.drawPairs
# pass
if settings.drawAABBs:
color = b2Color(0.9, 0.3, 0.9)
# cm = world.contactManager
for body in world.bodies:
if not body.active:
continue
transform = body.transform
for fixture in body.fixtures:
shape = fixture.shape
for childIndex in range(shape.childCount):
self.DrawAABB(shape.getAABB(
transform, childIndex), color)
class FrameworkBase(b2ContactListener):
"""
The base of the main testbed framework.
If you are planning on using the testbed framework and:
* Want to implement your own renderer (other than Pygame, etc.):
You should derive your class from this one to implement your own tests.
See empty.py or any of the other tests for more information.
* Do NOT want to implement your own renderer:
You should derive your class from Framework. The renderer chosen in
fwSettings (see settings.py) or on the command line will automatically
be used for your test.
"""
name = "None"
description = None
TEXTLINE_START = 30
colors = {
# 'static_body': b2Color(0.7, 1, 0.4),
'mouse_point': b2Color(0, 1, 0),
'joint_line': b2Color(0.8, 0.8, 0.8),
'contact_add': b2Color(0.3, 0.95, 0.3),
'contact_persist': b2Color(0.3, 0.3, 0.95),
'contact_normal': b2Color(0.4, 0.9, 0.4),
}
def __reset(self):
""" Reset all of the variables to their starting values.
Not to be called except at initialization."""
# Box2D-related
self.points = []
self.world = None
self.mouseJoint = None
self.settings = fwSettings
self.mouseWorld = None
self.using_contacts = False
self.stepCount = 0
# Box2D-callbacks
self.destructionListener = None
self.renderer = None
def __init__(self):
super(FrameworkBase, self).__init__()
self.__reset()
# Box2D Initialization
self.world = b2World(gravity=(0, -10), doSleep=True)
self.destructionListener = fwDestructionListener(test=self)
self.world.destructionListener = self.destructionListener
self.world.contactListener = self
self.t_steps, self.t_draws = [], []
def __del__(self):
pass
def Step(self, settings):
"""
The main physics step.
Takes care of physics drawing (callbacks are executed after the world.Step() )
and drawing additional information.
"""
self.stepCount += 1
# Don't do anything if the setting's Hz are <= 0
if settings.hz > 0.0:
timeStep = 1.0 / settings.hz
else:
timeStep = 0.0
renderer = self.renderer
# If paused, display so
if settings.pause:
if settings.singleStep:
settings.singleStep = False
else:
timeStep = 0.0
# Set the flags based on what the settings show
if renderer:
# convertVertices is only applicable when using b2DrawExtended. It
# indicates that the C code should transform box2d coords to screen
# coordinates.
is_extended = isinstance(renderer, b2DrawExtended)
renderer.flags = dict(
drawShapes=settings.drawShapes,
drawJoints=settings.drawJoints,
drawAABBs=settings.drawAABBs,
drawPairs=settings.drawPairs,
drawCOMs=settings.drawCOMs,
convertVertices=is_extended,
)
# Set the other settings that aren't contained in the flags
self.world.warmStarting = settings.enableWarmStarting
self.world.continuousPhysics = settings.enableContinuous
self.world.subStepping = settings.enableSubStepping
# Reset the collision points
self.points = []
# Tell Box2D to step
t_step = time()
self.world.Step(timeStep, settings.velocityIterations,
settings.positionIterations)
self.world.ClearForces()
t_step = time() - t_step
# Update the debug draw settings so that the vertices will be properly
# converted to screen coordinates
t_draw = time()
if renderer is not None:
renderer.StartDraw()
self.world.DrawDebugData()
# Take care of additional drawing (fps, mouse joint, slingshot bomb,
# contact points)
if renderer:
# If there's a mouse joint, draw the connection between the object
# and the current pointer position.
if self.mouseJoint:
p1 = renderer.to_screen(self.mouseJoint.anchorB)
p2 = renderer.to_screen(self.mouseJoint.target)
renderer.DrawPoint(p1, settings.pointSize,
self.colors['mouse_point'])
renderer.DrawPoint(p2, settings.pointSize,
self.colors['mouse_point'])
renderer.DrawSegment(p1, p2, self.colors['joint_line'])
renderer.EndDraw()
t_draw = time() - t_draw
t_draw = max(b2_epsilon, t_draw)
t_step = max(b2_epsilon, t_step)
try:
self.t_draws.append(1.0 / t_draw)
except:
pass
else:
if len(self.t_draws) > 2:
self.t_draws.pop(0)
try:
self.t_steps.append(1.0 / t_step)
except:
pass
else:
if len(self.t_steps) > 2:
self.t_steps.pop(0)
def MouseDown(self, p):
"""
Indicates that there was a left click at point p (world coordinates)
"""
if self.mouseJoint is not None:
return
# Create a mouse joint on the selected body (assuming it's dynamic)
# Make a small box.
aabb = b2AABB(lowerBound=p - (0.001, 0.001),
upperBound=p + (0.001, 0.001))
# Query the world for overlapping shapes.
query = fwQueryCallback(p)
self.world.QueryAABB(query, aabb)
if query.fixture:
body = query.fixture.body
# A body was selected, create the mouse joint
self.mouseJoint = self.world.CreateMouseJoint(
bodyA=self.groundbody,
bodyB=body,
target=p,
maxForce=(100.0**3) * body.mass)
body.awake = True
def rotateLeft(self, p):
"""
Indicates that there was a left click at point p (world coordinates)
"""
# if self.mouseJoint is not None:
# return
# Create a mouse joint on the selected body (assuming it's dynamic)
# Make a small box.
aabb = b2AABB(lowerBound=p - (0.001, 0.001),
upperBound=p + (0.001, 0.001))
# Query the world for overlapping shapes.
query = fwQueryCallback(p)
self.world.QueryAABB(query, aabb)
if query.fixture:
body = query.fixture.body
body.ApplyAngularImpulse(15**3, True)
def rotateRight(self, p):
"""
Indicates that there was a left click at point p (world coordinates)
"""
# if self.mouseJoint is not None:
# return
# Create a mouse joint on the selected body (assuming it's dynamic)
# Make a small box.
aabb = b2AABB(lowerBound=p - (0.001, 0.001),
upperBound=p + (0.001, 0.001))
# Query the world for overlapping shapes.
query = fwQueryCallback(p)
self.world.QueryAABB(query, aabb)
if query.fixture:
body = query.fixture.body
body.ApplyAngularImpulse(-15**3, True)
def MouseUp(self, p):
"""
Left mouse button up.
"""
if self.mouseJoint:
self.world.DestroyJoint(self.mouseJoint)
self.mouseJoint = None
def MouseMove(self, p):
"""
Mouse moved to point p, in world coordinates.
"""
self.mouseWorld = p
if self.mouseJoint:
self.mouseJoint.target = p
def SimulationLoop(self):
"""
The main simulation loop. Don't override this, override Step instead.
"""
self.Step(self.settings)
def ConvertScreenToWorld(self, x, y):
"""
Return a b2Vec2 in world coordinates of the passed in screen
coordinates x, y
NOTE: Renderer subclasses must implement this
"""
raise NotImplementedError()
def DrawStringAt(self, x, y, str, color=(229, 153, 153, 255)):
"""
Draw some text, str, at screen coordinates (x, y).
NOTE: Renderer subclasses must implement this
"""
raise NotImplementedError()
def PreSolve(self, contact, old_manifold):
"""
This is a critical function when there are many contacts in the world.
It should be optimized as much as possible.
"""
if not (self.settings.drawContactPoints
or self.settings.drawContactNormals or self.using_contacts):
return
elif len(self.points) > self.settings.maxContactPoints:
return
manifold = contact.manifold
if manifold.pointCount == 0:
return
state1, state2 = b2GetPointStates(old_manifold, manifold)
if not state2:
return
worldManifold = contact.worldManifold
# TODO: find some way to speed all of this up.
self.points.extend([
dict(
fixtureA=contact.fixtureA,
fixtureB=contact.fixtureB,
position=worldManifold.points[i],
normal=worldManifold.normal.copy(),
state=state2[i],
) for i, point in enumerate(state2)
])
def destroy_bodies(self):
"""
Finish the bodies into world when quit button is pressed
"""
for body in self.world.bodies:
self.world.DestroyBody(body)
# These can/should be implemented in the test subclass: (Step() also if necessary)
# See empty.py for a simple example.
def BeginContact(self, contact):
pass
def EndContact(self, contact):
pass
def PostSolve(self, contact, impulse):
pass
def FixtureDestroyed(self, fixture):
"""
Callback indicating 'fixture' has been destroyed.
"""
pass
def JointDestroyed(self, joint):
"""
Callback indicating 'joint' has been destroyed.
"""
pass
def Keyboard(self, key):
"""
Callback indicating 'key' has been pressed down.
"""
pass
def KeyboardUp(self, key):
"""
Callback indicating 'key' has been released.
"""
pass
