def draw(self, world, init_x, init_y, force_to_center):
HULL_FIXTURES = [
fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
y / self.SCALE)
for x, y in polygon]),
density=self.DENSITY * 1.25,
friction=0.1,
categoryBits=0x20,
maskBits=0x000F) # 0.99 bouncy
for polygon in HULL_POLYGONS
]
LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
self.LEG_H / 2)),
density=self.DENSITY * 0.25,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
self.LEG_H / 2)),
density=self.DENSITY * 0.25,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
hull = world.CreateDynamicBody(position=(init_x, init_y),
fixtures=HULL_FIXTURES)
hull.color1 = (0.44, 0.81, 0.14)
hull.color2 = (0.36, 0.66, 0.11)
hull.ApplyForceToCenter((force_to_center, 0), True)
hull.userData = CustomBodyUserData(True,
is_contact_critical=True,
name="hull")
self.body_parts.append(hull)
self.reference_head_object = hull
for i in [-1, +1]:
leg = world.CreateDynamicBody(
position=(init_x, init_y - self.LEG_H / 2 - self.LEG_DOWN),
#angle=(i * 0.05),#2°
fixtures=LEG_FD)
leg.color1 = (0.44, 0.81, 0.14)
leg.color2 = (0.36, 0.66, 0.11)
rjd = revoluteJointDef(
bodyA=hull,
bodyB=leg,
anchor=(init_x, init_y - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=i,
lowerAngle=-0.8,
upperAngle=1.1,
)
leg.userData = CustomBodyUserData(False, name="leg")
self.body_parts.append(leg)
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.LEG_H * 3 / 2 - self.LEG_DOWN),
#angle=(i * 0.05), #2°
fixtures=LOWER_FD)
lower.color1 = (1.0, 0.25, 0.04)
lower.color2 = (0.86, 0.29, 0.12)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
anchor=(init_x, init_y - self.LEG_DOWN - self.LEG_H),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.6,
upperAngle=-0.1,
)
lower.userData = CustomBodyUserData(True, name="lower")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
True,
contact_body=lower,
angle_correction=1.0)
self.motors.append(joint_motor)
def draw(self, world, init_x, init_y, force_to_center):
MAIN_BODY_FIXTURES = [
fixtureDef(
shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in polygon]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x000F)
for polygon in MAIN_BODY_POLYGONS
]
LEG_FD = fixtureDef(
shape=polygonShape(box=(self.LEG_W / 2, self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
LOWER_FD = fixtureDef(
shape=polygonShape(box=(0.8 * self.LEG_W / 2, self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
init_x = init_x - MAIN_BODY_BOTTOM_WIDTH / self.SCALE * self.nb_of_bodies / 2 # initial init_x is the middle of the whole body
previous_main_body = None
for j in range(self.nb_of_bodies):
main_body_x = init_x + j * (MAIN_BODY_BOTTOM_WIDTH / self.SCALE)
main_body = world.CreateDynamicBody(
position=(main_body_x, init_y),
fixtures=MAIN_BODY_FIXTURES
)
main_body.color1 = (0.5, 0.4, 0.9)
main_body.color2 = (0.3, 0.3, 0.5)
main_body.ApplyForceToCenter((force_to_center, 0), True)
# self.body_parts.append(BodyPart(main_body, True, True))
main_body.userData = CustomBodyUserData(True, is_contact_critical=False, name="body")
self.body_parts.append(main_body)
for i in [-1, +1]:
leg = world.CreateDynamicBody(
position=(main_body_x, init_y - self.LEG_H / 2 - self.LEG_DOWN),
#angle=(i * 0.05),
fixtures=LEG_FD
)
leg.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
leg.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=main_body,
bodyB=leg,
anchor=(main_body_x, init_y - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=i,
lowerAngle=-0.8,
upperAngle=1.1,
)
leg.userData = CustomBodyUserData(False, name="leg")
self.body_parts.append(leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
lower = world.CreateDynamicBody(
position=(main_body_x, init_y - self.LEG_H * 3 / 2 - self.LEG_DOWN),
#angle=(i * 0.05),
fixtures=LOWER_FD
)
lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
anchor=(main_body_x, init_y - self.LEG_DOWN - self.LEG_H),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.6,
upperAngle=-0.1,
)
lower.userData = CustomBodyUserData(True, name="lower")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(
SPEED_KNEE,
True,
contact_body=lower,
angle_correction=1.0)
self.motors.append(joint_motor)
if previous_main_body is not None:
rjd = revoluteJointDef(
bodyA=previous_main_body,
bodyB=main_body,
anchor=(main_body_x - MAIN_BODY_BOTTOM_WIDTH / self.SCALE / 2, init_y),
enableMotor=False,
enableLimit=True,
lowerAngle=-0.05 * np.pi,
upperAngle=0.05 * np.pi,
)
world.CreateJoint(rjd)
previous_main_body = main_body
# TODO : Check if this is the best way
self.reference_head_object = previous_main_body # set as head the rightmost body
def draw(self, world, init_x, init_y, force_to_center):
''' Circular body
MAIN_BODY_FIXTURES = fixtureDef(
shape=circleShape(radius=0.4),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F
)
'''
MAIN_BODY_FIXTURES = [
fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
y / self.SCALE)
for x, y in polygon]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x000F) for polygon in MAIN_BODY_POLYGONS
]
LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
main_body = world.CreateDynamicBody(position=(init_x, init_y),
fixtures=MAIN_BODY_FIXTURES)
basis_color1 = (0.6, 0.3, 0.5)
basis_color2 = (0.4, 0.2, 0.3)
main_body.color1 = tuple([c - 0.1 for c in basis_color1])
main_body.color2 = tuple([c - 0.1 for c in basis_color2])
leg_color1 = tuple([c + 0.1 for c in basis_color1])
leg_color2 = tuple([c + 0.1 for c in basis_color2])
main_body.ApplyForceToCenter((force_to_center, 0), True)
main_body.userData = CustomBodyUserData(True,
is_contact_critical=True,
name="main_body")
self.body_parts.append(main_body)
self.reference_head_object = main_body
for i in [+1, -1] * self.nb_pairs_of_legs:
##### First part of the leg #####
upper_leg_angle = 0.25 * np.pi * i
upper_leg_x_distance = np.sin(upper_leg_angle) * self.LEG_H / 2
upper_leg_y_distance = np.cos(upper_leg_angle) * self.LEG_H / 2
upper_leg_x = init_x - i * MAIN_BODY_BOTTOM_WIDTH / self.SCALE / 2 - upper_leg_x_distance
upper_leg_y = init_y + upper_leg_y_distance - self.LEG_DOWN
upper_leg = world.CreateDynamicBody(position=(upper_leg_x,
upper_leg_y),
angle=upper_leg_angle,
fixtures=LEG_FD)
upper_leg.color1 = leg_color1
upper_leg.color2 = leg_color2
rjd = revoluteJointDef(
bodyA=main_body,
bodyB=upper_leg,
anchor=(init_x - i * MAIN_BODY_BOTTOM_WIDTH / self.SCALE / 2,
init_y - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.1 * np.pi,
upperAngle=0.1 * np.pi,
)
upper_leg.userData = CustomBodyUserData(False, name="upper_leg")
self.body_parts.append(upper_leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
##### Second part of the leg #####
middle_leg_angle = 0.7 * np.pi * i
middle_leg_x_distance = np.sin(middle_leg_angle) * self.LEG_H / 2
middle_leg_y_distance = -np.cos(middle_leg_angle) * self.LEG_H / 2
middle_leg_x = upper_leg_x - upper_leg_x_distance - middle_leg_x_distance
middle_leg_y = upper_leg_y + upper_leg_y_distance - middle_leg_y_distance
middle_leg = world.CreateDynamicBody(position=(middle_leg_x,
middle_leg_y),
angle=middle_leg_angle,
fixtures=LEG_FD)
middle_leg.color1 = leg_color1
middle_leg.color2 = leg_color2
rjd = revoluteJointDef(
bodyA=upper_leg,
bodyB=middle_leg,
anchor=(upper_leg_x - upper_leg_x_distance,
upper_leg_y + upper_leg_y_distance),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.15 * np.pi,
upperAngle=0.15 * np.pi,
)
middle_leg.userData = CustomBodyUserData(False, name="middle_leg")
self.body_parts.append(middle_leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
##### Third part of the leg #####
lower_leg_angle = 0.9 * np.pi * i
lower_leg_x_distance = np.sin(lower_leg_angle) * self.LEG_H / 2
lower_leg_y_distance = -np.cos(lower_leg_angle) * self.LEG_H / 2
lower_leg_x = middle_leg_x - middle_leg_x_distance - lower_leg_x_distance
lower_leg_y = middle_leg_y - middle_leg_y_distance - lower_leg_y_distance
lower_leg = world.CreateDynamicBody(position=(lower_leg_x,
lower_leg_y),
angle=lower_leg_angle,
fixtures=LOWER_FD)
lower_leg.color1 = leg_color1
lower_leg.color2 = leg_color2
rjd = revoluteJointDef(
bodyA=middle_leg,
bodyB=lower_leg,
anchor=(middle_leg_x - middle_leg_x_distance,
middle_leg_y - middle_leg_y_distance),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.2 * np.pi,
upperAngle=0.2 * np.pi,
)
lower_leg.userData = CustomBodyUserData(True, name="lower_leg")
self.body_parts.append(lower_leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
True,
contact_body=lower_leg)
self.motors.append(joint_motor)
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 draw(self, world, init_x, init_y, force_to_center):
HULL_FIXTURES = [
fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
y / self.SCALE)
for x, y in polygon]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x000F) for polygon in HULL_POLYGONS
]
LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
hull = world.CreateDynamicBody(position=(init_x, init_y),
fixtures=HULL_FIXTURES)
hull.color1 = (0.5, 0.4, 0.9)
hull.color2 = (0.3, 0.3, 0.5)
hull.ApplyForceToCenter((force_to_center, 0), True)
hull.userData = CustomBodyUserData(True,
is_contact_critical=True,
name="hull")
self.body_parts.append(hull)
self.reference_head_object = hull
for x_anchor in [-1.2, 0.85]:
absolute_x = init_x + np.interp(x_anchor, [-1, 1], [
-HULL_BOTTOM_WIDTH / 2 / self.SCALE,
HULL_BOTTOM_WIDTH / 2 / self.SCALE
])
for i in [-1, +1]:
leg = world.CreateDynamicBody(
position=(absolute_x,
init_y - self.LEG_H / 2 - self.LEG_DOWN),
angle=(i * 0.05),
fixtures=LEG_FD)
leg.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
leg.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=hull,
bodyB=leg,
localAnchorA=(x_anchor, self.LEG_DOWN),
localAnchorB=(0, self.LEG_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=i,
lowerAngle=-0.8,
upperAngle=1.1,
)
leg.userData = CustomBodyUserData(False, name="leg")
self.body_parts.append(leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
lower = world.CreateDynamicBody(
position=(absolute_x,
init_y - self.LEG_H * 3 / 2 - self.LEG_DOWN),
angle=(i * 0.05),
fixtures=LOWER_FD)
lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
localAnchorA=(0, -self.LEG_H / 2),
localAnchorB=(0, self.LEG_H / 2),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.6,
upperAngle=-0.1,
)
lower.userData = CustomBodyUserData(True, name="lower")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(
SPEED_KNEE, True, contact_body=lower, angle_correction=1.0)
self.motors.append(joint_motor)
def draw(self, world, init_x, init_y, force_to_center):
''' Circular body
MAIN_BODY_FIXTURES = fixtureDef(
shape=circleShape(radius=0.4),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F
)
'''
MAIN_BODY_FIXTURES = [
fixtureDef(
shape=polygonShape(vertices=[(x * self.body_scale / self.SCALE,
y * self.body_scale / self.SCALE)
for x, y in polygon]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x000F) for polygon in MAIN_BODY_POLYGONS
]
LEG_FD = fixtureDef(shape=polygonShape(box=(self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
LOWER_FD = fixtureDef(shape=polygonShape(box=(0.8 * self.LEG_W / 2,
self.LEG_H / 2)),
density=1.0,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
main_body = world.CreateDynamicBody(position=(init_x, init_y),
fixtures=MAIN_BODY_FIXTURES)
basis_color1 = (0.6, 0.3, 0.5)
basis_color2 = (0.4, 0.2, 0.3)
main_body.color1 = tuple([c - 0.1 for c in basis_color1])
main_body.color2 = tuple([c - 0.1 for c in basis_color2])
leg_color1 = tuple([c + 0.1 for c in basis_color1])
leg_color2 = tuple([c + 0.1 for c in basis_color2])
main_body.ApplyForceToCenter((force_to_center, 0), True)
main_body.userData = CustomBodyUserData(True,
is_contact_critical=True,
name="main_body")
self.body_parts.append(main_body)
self.reference_head_object = main_body
for j in [[0, -1, 0], [0, 1, 0], [-1, 0, np.pi / 2], [1, 0,
np.pi / 2]]:
x_position = j[0] * (HULL_BOTTOM_WIDTH * self.body_scale /
self.SCALE / 2)
y_position = j[1] * (HULL_BOTTOM_WIDTH * self.body_scale /
self.SCALE / 2)
# for i in [-1, +1]:
leg_x = init_x + j[0] * (self.LEG_H / 2) + x_position
leg_y = init_y + j[1] * (self.LEG_H / 2) + y_position
leg = world.CreateDynamicBody(position=(leg_x, leg_y),
angle=(j[2]),
fixtures=LEG_FD)
leg.color1 = leg_color1
leg.color2 = leg_color2
rjd = revoluteJointDef(
bodyA=main_body,
bodyB=leg,
anchor=(init_x + x_position, init_y + y_position),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.8,
upperAngle=1.1,
)
leg.userData = CustomBodyUserData(False, name="leg")
self.body_parts.append(leg)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_HIP, False)
self.motors.append(joint_motor)
lower = world.CreateDynamicBody(
position=(leg_x + j[0] * (self.LEG_H),
leg_y + j[1] * (self.LEG_H)),
angle=(j[2]),
fixtures=LOWER_FD)
lower.color1 = leg_color1
lower.color2 = leg_color2
rjd = revoluteJointDef(
bodyA=leg,
bodyB=lower,
anchor=(leg_x + j[0] * (self.LEG_H / 2),
leg_y + j[1] * (self.LEG_H / 2)),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-1.6,
upperAngle=-0.1,
)
lower.userData = CustomBodyUserData(True, name="lower")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
True,
contact_body=lower,
angle_correction=1.0)
self.motors.append(joint_motor)
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 [(-8, +25), (+8,
+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)
# LEGS
for i in [+1, +1]:
upper = world.CreateDynamicBody(
position=(init_x, init_y - self.LIMB_H / 2 - self.LEG_DOWN),
# angle=(i * 0.05),
fixtures=UPPER_LIMB_FD)
upper.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
upper.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=body,
bodyB=upper,
anchor=(init_x, init_y - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.3 * np.pi,
upperAngle=0.6 * np.pi,
)
upper.userData = CustomBodyUserData(False, name="upper_leg")
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.LEG_DOWN),
# angle=(i * 0.05),
fixtures=LOWER_LIMB_FD)
lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=upper,
bodyB=lower,
anchor=(init_x, init_y - self.LIMB_H - self.LEG_DOWN),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.75 * np.pi,
upperAngle=-0.1,
)
lower.userData = CustomBodyUserData(True, name="lower_leg")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
True,
angle_correction=1.0,
contact_body=lower)
self.motors.append(joint_motor)
# 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.5 * np.pi,
upperAngle=0.8 * np.pi,
)
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(True, name="lowar_arm")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
True,
contact_body=lower)
self.motors.append(joint_motor)
def draw(self, world, init_x, init_y, force_to_center):
init_y = init_y + 1
#### HULL ####
HULL_FD = fixtureDef(
shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in HULL_POLYGONS]),
density=self.DENSITY,
friction=0.1,
categoryBits=0x20,
maskBits=0x000F) # 0.99 bouncy
hull = world.CreateDynamicBody(
position=(init_x, init_y),
fixtures=HULL_FD
)
hull.color1 = (0.5, 0.4, 0.9)
hull.color2 = (0.3, 0.3, 0.5)
# hull.ApplyForceToCenter((force_to_center, 0), True)
hull.userData = CustomBodyUserData(True, is_contact_critical=False, name="head")
self.body_parts.append(hull)
self.reference_head_object = hull
#### P1 ####
body_p1_x = init_x - 35 / 2 / self.SCALE - 16 / 2 / self.SCALE
BODY_P1_FD = fixtureDef(
shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in BODY_P1]),
density=self.DENSITY,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
body_p1 = world.CreateDynamicBody(
position=(body_p1_x, init_y),
fixtures=BODY_P1_FD
)
body_p1.color1 = (0.5, 0.4, 0.9)
body_p1.color2 = (0.3, 0.3, 0.5)
rjd = revoluteJointDef(
bodyA=hull,
bodyB=body_p1,
anchor=(init_x - 35 / 2 / self.SCALE, init_y),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.1 * np.pi,
upperAngle=0.2 * np.pi,
)
body_p1.userData = CustomBodyUserData(True, name="body")
self.body_parts.append(body_p1)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE, True, contact_body=body_p1)
self.motors.append(joint_motor)
#### P2 ####
body_p2_x = body_p1_x - 16 / 2 / self.SCALE - 16 / 2 / self.SCALE
BODY_P2_FD = fixtureDef(
shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in BODY_P2]),
density=self.DENSITY,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
body_p2 = world.CreateDynamicBody(
position=(body_p2_x, init_y),
fixtures=BODY_P2_FD
)
body_p2.color1 = (0.5, 0.4, 0.9)
body_p2.color2 = (0.3, 0.3, 0.5)
rjd = revoluteJointDef(
bodyA=body_p1,
bodyB=body_p2,
anchor=(body_p1_x - 16 / 2 / self.SCALE, init_y),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.15 * np.pi,
upperAngle=0.15 * np.pi,
)
body_p2.userData = CustomBodyUserData(True, name="body")
self.body_parts.append(body_p2)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE, True, contact_body=body_p2)
self.motors.append(joint_motor)
#### P3 ####
body_p3_x = body_p2_x - 16 / 2 / self.SCALE - 8 / 2 / self.SCALE
BODY_P3_FD = fixtureDef(
shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in BODY_P3]),
density=self.DENSITY,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
body_p3 = world.CreateDynamicBody(
position=(body_p3_x, init_y),
fixtures=BODY_P3_FD
)
body_p3.color1 = (0.5, 0.4, 0.9)
body_p3.color2 = (0.3, 0.3, 0.5)
rjd = revoluteJointDef(
bodyA=body_p2,
bodyB=body_p3,
anchor=(body_p2_x - 16 / 2 / self.SCALE, init_y),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.3 * np.pi,
upperAngle=0.3 * np.pi,
)
body_p3.userData = CustomBodyUserData(True, name="body")
self.body_parts.append(body_p3)
self.tail = body_p3
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE, True, contact_body=body_p3)
self.motors.append(joint_motor)
#### FIN ####
FIN_FD = fixtureDef(
shape=polygonShape(vertices=[(x / self.SCALE, y / self.SCALE) for x, y in FIN]),
density=self.DENSITY,
restitution=0.0,
categoryBits=0x20,
maskBits=0x000F)
fin_positions = [
# [init_x + 35 / 2 / self.SCALE / 2, init_y],
# [init_x - 35 / 2 / self.SCALE / 2, init_y],
[init_x, init_y - 22 / 2 / self.SCALE + 0.2],
# [init_x - 35 / 2 / self.SCALE / 2, init_y - 22 / 2 / self.SCALE + 0.1],
]
fin_angle = -0.2 * np.pi
middle_fin_x_distance = np.sin(fin_angle) * 20 / 2 / self.SCALE
middle_fin_y_distance = np.cos(fin_angle) * 20 / 2 / self.SCALE
for fin_pos in fin_positions:
current_fin_x = fin_pos[0] + middle_fin_x_distance
current_fin_y = fin_pos[1] - middle_fin_y_distance
fin = world.CreateDynamicBody(
position=(current_fin_x, current_fin_y),
fixtures=FIN_FD,
angle=fin_angle
)
fin.color1 = (0.5, 0.4, 0.9)
fin.color2 = (0.3, 0.3, 0.5)
rjd = revoluteJointDef(
bodyA=hull,
bodyB=fin,
anchor=(fin_pos[0], fin_pos[1]),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=-0.3 * np.pi,
upperAngle=0.2 * np.pi,
)
fin.userData = CustomBodyUserData(True, name="fin")
self.body_parts.append(fin)
self.fins.append(fin)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE, True, contact_body=fin)
self.motors.append(joint_motor)
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_POLYGONS = [[(-20, +25), (+20, +25), (+4, -20), (-4, -20)],
[(-15, -20), (+15, -20), (-15, -22), (+15, -22)]]
BODY_FIXTURES = [
fixtureDef(shape=polygonShape(vertices=[(x / self.SCALE,
y / self.SCALE)
for x, y in polygon]),
density=5.0,
friction=0.1,
categoryBits=0x20,
maskBits=0x1) for polygon in BODY_POLYGONS
]
body = world.CreateDynamicBody(position=(init_x, init_y),
fixtures=BODY_FIXTURES)
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)
# Left / Right side of body
for i in [-1, +1]:
current_x = init_x + i * self.BODY_WIDTH / self.SCALE / 2
# LEG
leg_top_y = init_y - self.BODY_HEIGHT / self.SCALE / 2
upper = world.CreateDynamicBody(position=(current_x, leg_top_y -
self.LIMB_H / 2),
fixtures=UPPER_LIMB_FD)
upper.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
upper.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=body,
bodyB=upper,
anchor=(current_x, leg_top_y),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=int(i < 0) * -0.75 * np.pi,
upperAngle=int(i > 0) * 0.75 * np.pi,
)
upper.userData = CustomBodyUserData(False, name="upper_leg")
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=(current_x, leg_top_y -
self.LIMB_H * 3 / 2),
fixtures=LOWER_LIMB_FD)
lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=upper,
bodyB=lower,
anchor=(current_x, leg_top_y - self.LIMB_H),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=int(i > 0) * -0.7 * np.pi,
upperAngle=int(i < 0) * 0.7 * np.pi,
)
lower.userData = CustomBodyUserData(True, name="lower_leg")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE,
True,
contact_body=lower,
angle_correction=1.0)
self.motors.append(joint_motor)
# ARM
arm_top_y = init_y + self.BODY_HEIGHT / self.SCALE / 2
upper = world.CreateDynamicBody(position=(current_x, arm_top_y -
self.LIMB_H / 2),
fixtures=UPPER_LIMB_FD)
upper.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
upper.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=body,
bodyB=upper,
anchor=(current_x, arm_top_y),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=int(i < 0) * -0.9 * np.pi,
upperAngle=int(i > 0) * 0.9 * np.pi,
)
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=(current_x, arm_top_y -
self.LIMB_H * 3 / 2),
fixtures=LOWER_LIMB_FD)
lower.color1 = (0.6 - i / 10., 0.3 - i / 10., 0.5 - i / 10.)
lower.color2 = (0.4 - i / 10., 0.2 - i / 10., 0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=upper,
bodyB=lower,
anchor=(current_x, arm_top_y - self.LIMB_H),
enableMotor=True,
enableLimit=True,
maxMotorTorque=self.MOTORS_TORQUE,
motorSpeed=1,
lowerAngle=int(i < 0) * -0.8 * np.pi,
upperAngle=int(i > 0) * 0.8 * np.pi,
)
lower.userData = CustomBodyUserData(False, name="lower_arm")
self.body_parts.append(lower)
joint_motor = world.CreateJoint(rjd)
joint_motor.userData = CustomMotorUserData(SPEED_KNEE, False)
self.motors.append(joint_motor)
# hand
prev_part = lower
initial_hand_y = arm_top_y - self.LIMB_H * 2
angle_boundaries = [[-0.4, 0.4], [-0.5, 0.5], [-0.8, 0.8]]
for u in range(3):
hand_part = world.CreateDynamicBody(
position=(current_x, initial_hand_y -
self.HAND_PART_H / 2 - self.HAND_PART_H * u),
fixtures=HAND_PART_FD)
hand_part.color1 = (0.6 - i / 10., 0.3 - i / 10.,
0.5 - i / 10.)
hand_part.color2 = (0.4 - i / 10., 0.2 - i / 10.,
0.3 - i / 10.)
rjd = revoluteJointDef(
bodyA=prev_part,
bodyB=hand_part,
anchor=(current_x, initial_hand_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