def draw(self, world, init_x, init_y, force_to_center):
head = world.CreateDynamicBody(
position=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2 +
self.HEAD_HEIGHT / self.SCALE / 2 + 0.2),
fixtures=fixtureDef(shape=polygonShape(
vertices=[(x / self.SCALE, y / self.SCALE)
for x, y in [(-5, +10), (+5, +10), (+5,
-10), (-5,
-10)]]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x1))
head.color1 = (0.5, 0.4, 0.9)
head.color2 = (0.3, 0.3, 0.5)
head.ApplyForceToCenter((force_to_center, 0), True)
head.userData = CustomBodyUserData(True,
is_contact_critical=True,
name="head")
self.body_parts.append(head)
self.reference_head_object = head
body = world.CreateDynamicBody(
position=(init_x, init_y),
fixtures=fixtureDef(
shape=polygonShape(
vertices=[(x / self.SCALE, y / self.SCALE)
for x, y in [(-12, +25), (+12,
+25), (+8,
-20), (-8,
-20)]]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x1 # collide only with ground
))
body.color1 = (0.5, 0.4, 0.9)
body.color2 = (0.3, 0.3, 0.5)
body.userData = CustomBodyUserData(True,
is_contact_critical=True,
name="body")
self.body_parts.append(body)
rjd = revoluteJointDef(
bodyA=head,
bodyB=body,
anchor=(init_x, init_y + self.BODY_HEIGHT / self.SCALE / 2),
enableMotor=False,
enableLimit=True,
lowerAngle=-0.1 * np.pi,
upperAngle=0.1 * np.pi,
)
world.CreateJoint(rjd)
UPPER_LIMB_FD = fixtureDef(shape=polygonShape(box=(self.LIMB_W / 2,
self.LIMB_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x1)
LOWER_LIMB_FD = fixtureDef(
shape=polygonShape(box=(0.8 * self.LIMB_W / 2, self.LIMB_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x1)
HAND_PART_FD = fixtureDef(
shape=polygonShape(box=(self.HAND_PART_W / 2,
self.HAND_PART_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x1)
# ARMS
for j in [-1, -1]:
upper = world.CreateDynamicBody(
position=(init_x, init_y + self.LIMB_H / 2 + self.ARM_UP),
# angle=(i * 0.05),
fixtures=UPPER_LIMB_FD)
upper.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
upper.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
rjd = revoluteJointDef(
bodyA=body,
bodyB=upper,
anchor=(init_x, init_y + self.ARM_UP),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.75 * 2 * np.pi,
upperAngle=0,
)
upper.userData = CustomBodyUserData(False, name="upper_arm")
self.body_parts.append(upper)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
lower = world.CreateDynamicBody(
position=(init_x, init_y + self.LIMB_H * 3 / 2 + self.ARM_UP),
# angle=(i * 0.05),
fixtures=LOWER_LIMB_FD)
lower.color1 = (0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
lower.color2 = (0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
rjd = revoluteJointDef(
bodyA=upper,
bodyB=lower,
anchor=(init_x, init_y + self.LIMB_H + self.ARM_UP),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=0,
upperAngle=0.75 * np.pi,
)
lower.userData = CustomBodyUserData(False, name="lower_arm")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
# hand
prev_part = lower
initial_y = init_y + self.LIMB_H * 2 + self.ARM_UP
angle_boundaries = [[-0.5, 0.5]]
nb_hand_parts = 1
for u in range(nb_hand_parts):
hand_part = world.CreateDynamicBody(
position=(init_x, initial_y + self.HAND_PART_H / 2 +
self.HAND_PART_H * u),
fixtures=HAND_PART_FD)
hand_part.color1 = (0.6 - j / 10., 0.3 - j / 10.,
0.5 - j / 10.)
hand_part.color2 = (0.4 - j / 10., 0.2 - j / 10.,
0.3 - j / 10.)
rjd = revoluteJointDef(
bodyA=prev_part,
bodyB=hand_part,
anchor=(init_x, initial_y + self.HAND_PART_H * u),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=angle_boundaries[u][0] * np.pi,
upperAngle=angle_boundaries[u][1] * np.pi,
)
hand_part.userData = CustomBodyUserData(True, name="hand")
self.body_parts.append(hand_part)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(
SPEED_HAND, True, contact_body=hand_part)
self.motors.append(joint_motor)
prev_part = hand_part
hand_sensor_position = (init_x, initial_y +
self.HAND_PART_H * nb_hand_parts)
hand_sensor_part = world.CreateDynamicBody(
position=hand_sensor_position,
fixtures=self.SENSOR_FD,
userData=CustomBodySensoUserData(True, False, "hand_sensor"))
hand_sensor_part.color1 = (
1, 0, 0) #(0.6 - j / 10., 0.3 - j / 10., 0.5 - j / 10.)
hand_sensor_part.color2 = (
1, 0, 0) #(0.4 - j / 10., 0.2 - j / 10., 0.3 - j / 10.)
self.sensors.append(hand_sensor_part)
world.CreateJoint(
weldJointDef(bodyA=prev_part,
bodyB=hand_sensor_part,
anchor=hand_sensor_position))
def reset(self):
self._destroy()
self.world.contactListener_bug_workaround = ContactDetector(self)
self.world.contactListener = self.world.contactListener_bug_workaround
self.game_over = False
self.prev_shaping = None
self.scroll = 0.0
self.lidar_render = 0
W = VIEWPORT_W / SCALE
H = VIEWPORT_H / SCALE
self._generate_terrain()
self._generate_clouds()
init_x = TERRAIN_STEP * TERRAIN_STARTPAD / 2
init_y = TERRAIN_HEIGHT + 3 * LEG_H
self.body = self.world.CreateDynamicBody(
position=(init_x, init_y),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in BODY_SHAPE[-1]]),
density=0.5,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
for shape in BODY_SHAPE[:-1]:
self.body.CreateFixture(
fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in shape]),
density=0.5,
friction=0.1,
categoryBits=0x0020,
maskBits=0x001, # collide only with ground
restitution=0.0) # 0.99 bouncy
)
self.body.color1 = (0.9, 0.9, 0.9)
self.body.color2 = (0.9, 0.9, 0.9)
self.body.ApplyForceToCenter(
(self.np_random.uniform(-INITIAL_RANDOM, INITIAL_RANDOM), 0), True)
self.details = []
detailsJoints = []
for color1, color2, shapes in BODY_DETAILS:
detail = self.world.CreateDynamicBody(
position=(init_x, init_y),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in shapes[0]]),
density=0.0001,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0))
for shape in shapes:
another = detail.CreateFixture(
fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in shape]),
density=0.0001,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0))
detail.color1 = color1
detail.color2 = color2
self.details.append(detail)
joint = weldJointDef(
bodyA=detail,
bodyB=self.body,
localAnchorA=(0, 0),
localAnchorB=(0, 0),
)
detailsJoints.append(self.world.CreateJoint(joint))
self.legs = []
self.joints = []
for i in [-1, +1]:
leg_upper = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H / 2 - LEG_DOWN),
angle=(i * 0.05),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in LEG_UPPER_SHAPE]),
density=0.5,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001))
leg_upper.color1 = (0.9, 0.9, 0.9)
leg_upper.color2 = (0.9, 0.9, 0.9)
rjd = revoluteJointDef(
bodyA=self.body,
bodyB=leg_upper,
localAnchorA=(0, LEG_DOWN),
localAnchorB=(0, LEG_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=2 * i,
lowerAngle=-0.8,
upperAngle=1.1,
)
self.joints.append(self.world.CreateJoint(rjd))
leg_lower = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H * 3 / 2 - LEG_DOWN),
angle=(i * 0.05),
fixtures=fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in LEG_LOWER_SHAPE]),
density=0.5,
restitution=0.0,
categoryBits=0x0020,
maskBits=0x001))
leg_lower.color1 = (0.9, 0.9, 0.9)
leg_lower.color2 = (0.9, 0.9, 0.9)
rjd = revoluteJointDef(
bodyA=leg_upper,
bodyB=leg_lower,
localAnchorA=(0, -LEG_H / 3),
localAnchorB=(0, LEG_H / 3),
enableMotor=True,
enableLimit=True,
maxMotorTorque=MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.6,
upperAngle=-0.1,
)
leg_lower.ground_contact = False
self.joints.append(self.world.CreateJoint(rjd))
detail_upper = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H / 2 - LEG_DOWN),
fixtures=fixtureDef(shape=polygonShape(
vertices=[(x / SCALE, y / SCALE)
for x, y in LEG_UPPER_DETAILS[0]]),
density=0.0001,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0))
for shape in LEG_UPPER_DETAILS[1:]:
another = detail_upper.CreateFixture(
fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in shape]),
density=0.0001,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0))
detail_upper.color1 = (0, 0, 0)
detail_upper.color2 = (0, 0, 0)
joint = weldJointDef(
bodyA=detail_upper,
bodyB=leg_upper,
localAnchorA=(0, 0),
localAnchorB=(0, 0),
)
detailsJoints.append(self.world.CreateJoint(joint))
detail_lower = self.world.CreateDynamicBody(
position=(init_x, init_y - LEG_H / 2 - LEG_DOWN),
fixtures=fixtureDef(shape=polygonShape(
vertices=[(x / SCALE, y / SCALE)
for x, y in LEG_LOWER_DETAILS[0]]),
density=0.0001,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0))
for shape in LEG_LOWER_DETAILS[1:]:
another = detail_lower.CreateFixture(
fixtureDef(
shape=polygonShape(vertices=[(x / SCALE, y / SCALE)
for x, y in shape]),
density=0.0001,
categoryBits=0x0020,
maskBits=0x001,
restitution=0.0))
detail_lower.color1 = (0, 0, 0)
detail_lower.color2 = (0, 0, 0)
joint = weldJointDef(
bodyA=detail_lower,
bodyB=leg_lower,
localAnchorA=(0, 0),
localAnchorB=(0, 0),
)
detailsJoints.append(self.world.CreateJoint(joint))
self.details.append(detail_lower)
self.details.append(detail_upper)
self.legs.append(leg_upper)
self.legs.append(leg_lower)
self.joints.extend(detailsJoints)
self.drawlist = self.terrain + self.legs[::-1] + [self.body
] + self.details
class LidarCallback(Box2D.b2.rayCastCallback):
def ReportFixture(self, fixture, point, normal, fraction):
if (fixture.filterData.categoryBits & 1) == 0:
return 1
self.p2 = point
self.fraction = fraction
return 0
self.lidar = [LidarCallback() for _ in range(10)]
return self.step(np.array([0, 0, 0, 0]))[0]
def reset(self):
self._destroy()
self.world.contactListener_bug_workaround = ContactDetector(self)
self.world.contactListener = self.world.contactListener_bug_workaround
self.game_over = False
self.prev_shaping = None
self.scroll = 0.0
self.lidar_render = 0
W = VIEWPORT_W / SCALE
H = VIEWPORT_H / SCALE
self._generate_terrain(self.hardcore)
self._generate_clouds()
# Process the fixtures
# PolygonShape: vertices
# EdgeShape: vertices
# CircleShape: radius
# friction, density, restitution, maskBits, categoryBits
self.fixtures = {}
for k in self.fixture_defs.keys():
if self.fixture_defs[k]['FixtureShape']['Type'] == 'PolygonShape' or \
self.fixture_defs[k]['FixtureShape']['Type'] == 'EdgeShape':
fixture_shape = polygonShape(
vertices=[(x / SCALE, y / SCALE)
for x, y in self.fixture_defs[k]['FixtureShape']
['Vertices']])
elif self.fixture_defs[k]['FixtureShape']['Type'] == 'CircleShape':
fixture_shape = circleShape(
radius=self.fixture_defs[k]['FixtureShape']['Radius'] /
SCALE)
else:
print("Invalid fixture type: " +
self.fixture_defs[k]['FixtureShape'])
assert (False)
self.fixtures[k] = fixtureDef(
shape=fixture_shape,
friction=self.fixture_defs[k]['Friction'],
density=self.fixture_defs[k]['Density'],
restitution=self.fixture_defs[k]['Restitution'],
maskBits=self.fixture_defs[k]['MaskBits'],
categoryBits=self.fixture_defs[k]['CategoryBits'])
# Process the dynamic bodies
# position, angle, fixture,
self.bodies = {}
for k in self.body_defs.keys():
if False and k == 'Hull':
self.bodies[k] = self.world.CreateStaticBody(
position=[
x / SCALE for x in self.body_defs[k]['Position']
],
angle=self.body_defs[k]['Angle'],
fixtures=[
self.fixtures[f]
for f in self.body_defs[k]['FixtureNames']
])
else:
self.bodies[k] = self.world.CreateDynamicBody(
position=[
x / SCALE for x in self.body_defs[k]['Position']
],
angle=self.body_defs[k]['Angle'],
fixtures=[
self.fixtures[f]
for f in self.body_defs[k]['FixtureNames']
])
self.bodies[k].color1 = self.body_defs[k]['Color1']
self.bodies[k].color2 = self.body_defs[k]['Color2']
self.bodies[k].can_touch_ground = self.body_defs[k][
'CanTouchGround']
self.bodies[k].ground_contact = False
self.bodies[k].depth = self.body_defs[k]['Depth']
self.bodies[k].connected_body = []
self.bodies[k].connected_joints = []
# Apply a force to the 'center' body
if k == 'Hull':
self.bodies[k].ApplyForceToCenter((self.np_random.uniform(
-self.body_defs[k]['InitialForceScale'],
self.body_defs[k]['InitialForceScale']), 0), True)
self.body_state_order = copy.deepcopy(list(self.bodies_to_report_keys))
self.all_bodies_order = copy.deepcopy(
self.body_state_order) + copy.deepcopy([
k for k in self.body_defs.keys()
if k not in self.body_state_order
])
for i in range(len(self.all_bodies_order)):
k = self.all_bodies_order[i]
self.bodies[k].index = i
# Process the joint motors
# bodyA, bodyB, localAnchorA, localAnchorB, enableMotor, enableLimit,
# maxMotorTorque, motorSpeed, lowerAngle, upperAngle
self.joints = {}
for k in self.joint_defs.keys():
self.joints[k] = self.world.CreateJoint(
revoluteJointDef(
bodyA=self.bodies[self.joint_defs[k]['BodyA']],
bodyB=self.bodies[self.joint_defs[k]['BodyB']],
localAnchorA=[
x / SCALE for x in self.joint_defs[k]['LocalAnchorA']
],
localAnchorB=[
x / SCALE for x in self.joint_defs[k]['LocalAnchorB']
],
enableMotor=self.joint_defs[k]['EnableMotor'],
enableLimit=self.joint_defs[k]['EnableLimit'],
maxMotorTorque=self.joint_defs[k]['MaxMotorTorque'],
motorSpeed=self.joint_defs[k]['MotorSpeed'],
lowerAngle=self.joint_defs[k]['LowerAngle'],
upperAngle=self.joint_defs[k]['UpperAngle']))
self.joints[k].depth = self.joint_defs[k]['Depth']
self.joints[k].connected_body = []
self.joints[k].connected_joints = []
# Process the body linkages
# bodyA, bodyB, anchor
self.linkages = {}
for k in self.linkage_defs.keys():
self.linkages[k] = self.world.CreateJoint(
weldJointDef(bodyA=self.bodies[self.linkage_defs[k]['BodyA']],
bodyB=self.bodies[self.linkage_defs[k]['BodyB']],
localAnchorA=[
x / SCALE
for x in self.linkage_defs[k]['LocalAnchorA']
],
localAnchorB=[
x / SCALE
for x in self.linkage_defs[k]['LocalAnchorB']
],
frequencyHz=self.linkage_defs[k]['FrequencyHz']))
# Joints we want to report state is a superset of joints we want to allow action
self.joint_action_order = copy.deepcopy(list(self.enabled_joints_keys))
if self.truncate_state:
self.joint_state_order = copy.deepcopy(self.joint_action_order) \
+ copy.deepcopy([k for k in self.joint_defs.keys()
if k not in self.joint_action_order and self.joint_defs[k]['ReportState']])
else:
self.joint_state_order = copy.deepcopy(self.joint_action_order) \
+ copy.deepcopy([k for k in self.joint_defs.keys()
if k not in self.joint_action_order])
self.all_joints_order = copy.deepcopy(
self.joint_state_order) + copy.deepcopy([
k for k in self.joint_defs.keys()
if k not in self.joint_state_order
])
for i in range(len(self.all_joints_order)):
k = self.all_joints_order[i]
self.joints[k].index = i
# Construct index links between bodies and joints
for k in self.joint_defs.keys():
self.bodies[self.joint_defs[k]['BodyA']].connected_body.append(
self.bodies[self.joint_defs[k]['BodyB']].index)
self.bodies[self.joint_defs[k]['BodyB']].connected_body.append(
self.bodies[self.joint_defs[k]['BodyA']].index)
self.bodies[self.joint_defs[k]['BodyA']].connected_joints.append(
self.joints[k].index)
self.bodies[self.joint_defs[k]['BodyB']].connected_joints.append(
self.joints[k].index)
self.joints[k].connected_body.append(
self.bodies[self.joint_defs[k]['BodyA']].index)
self.joints[k].connected_body.append(
self.bodies[self.joint_defs[k]['BodyB']].index)
# Construct index links between hull segments via linkages
for k in self.linkage_defs.keys():
self.bodies[self.linkage_defs[k]['BodyA']].connected_body.append(
self.bodies[self.linkage_defs[k]['BodyB']].index)
self.bodies[self.linkage_defs[k]['BodyB']].connected_body.append(
self.bodies[self.linkage_defs[k]['BodyA']].index)
# Construct index links between joints connected to same body
for k_joint in self.joints.keys():
for i_body in self.joints[k_joint].connected_body:
k_body = self.all_bodies_order[i_body]
for i_jointB in self.bodies[k_body].connected_joints:
# Avoid adding self-links
if self.joints[k_joint].index == i_jointB:
continue
# Keep uniqueness of lists
if i_jointB in self.joints[k_joint].connected_joints:
continue
self.joints[k_joint].connected_joints.append(i_jointB)
# Make sure hull is last
self.drawlist = self.terrain + list(self.bodies.values())
class LidarCallback(Box2D.b2.rayCastCallback):
def ReportFixture(self, fixture, point, normal, fraction):
if (fixture.filterData.categoryBits & 1) == 0:
return 1
self.p2 = point
self.fraction = fraction
return 0
self.lidar = [LidarCallback() for _ in range(10)]
ob, _, _, info = self.step(np.zeros(self.action_space.shape))
return ob, info