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