def __init__(self, space):
super().__init__()
shape = pymunk.Poly(self, ((-20, 7.5), (-5, 10), (20, 5), (20, -5), (-5, -10), (-20, -7.5)))
shape.collision_type = 1
shape.density = 0.01 # タイヤ側の動作しか計算していないので、車両側は軽くして影響を減らしました……。
shape.elasticity = 0.2
self.tire_lf = Tire(space, self, 500)
self.tire_rf = Tire(space, self, 500)
self.tire_lr = Tire(space, self, 300) # ドリフトするように、リア・タイヤのグリップを落としました。
self.tire_rr = Tire(space, self, 300)
self.tire_lf.position = ( 12.5, 12.5) # noqa: E201, E241
self.tire_rf.position = ( 12.5, -12.5) # noqa: E201
self.tire_lr.position = (-12.5, 15.0) # noqa: E241
self.tire_rr.position = (-12.5, -15.0)
pyvot_tire_lf = pymunk.PivotJoint(self, self.tire_lf, self.tire_lf.position, (0, 0))
pyvot_tire_rf = pymunk.PivotJoint(self, self.tire_rf, self.tire_rf.position, (0, 0))
pyvot_tire_lr = pymunk.PivotJoint(self, self.tire_lr, self.tire_lr.position, (0, 0))
pyvot_tire_rr = pymunk.PivotJoint(self, self.tire_rr, self.tire_rr.position, (0, 0))
rotation_tire_lf = pymunk.RotaryLimitJoint(self, self.tire_lf, -pi / 6, pi / 6)
rotation_tire_rf = pymunk.RotaryLimitJoint(self.tire_lf, self.tire_rf, 0, 0)
rotation_tire_lr = pymunk.RotaryLimitJoint(self, self.tire_lr, 0, 0)
rotation_tire_rr = pymunk.RotaryLimitJoint(self, self.tire_rr, 0, 0)
space.add(self, shape, pyvot_tire_lf, pyvot_tire_rf, pyvot_tire_lr, pyvot_tire_rr, rotation_tire_lf, rotation_tire_rf, rotation_tire_lr, rotation_tire_rr)
def add_robot(space, seat):
#define robot upper body and upper leg
ubt = Vec2d(3.5, 0)
robot_u_points = [
Vec2d(-4, 31.64) + ubt,
Vec2d(-4, 0) + ubt,
Vec2d(4, 0) + ubt,
Vec2d(4, 31.64) + ubt
]
robot_body = pymunk.Body()
robot_body.position = seat.position + (0, 3)
robot_u = pymunk.Poly(robot_body, robot_u_points)
robot_u.mass = 3.311
robot_u.color = THECOLORS["red"]
robot_u.filter = pymunk.ShapeFilter(mask=pymunk.ShapeFilter.ALL_MASKS ^ 1)
robot_u_leg = pymunk.Poly(seat, [(1, 3), (-10.5, 3), (-10.5, 8), (1, 8)])
robot_u_leg.color = THECOLORS["red"]
robot_u_leg.mass = 0.603
#robot_u_leg_extra = pymunk.Circle(seat,2,(5,0))
#robot_u_leg_extra.mass = 2
#define robot lower leg
robot_leg = pymunk.Body()
robot_leg.position = seat.position
robot_l_leg = pymunk.Poly(robot_leg, [(-10.5, 5), (-10.5, -11.8),
(-4.0, -11.8), (-4.0, 5)])
robot_l_leg.mass = 1.214
robot_l_leg.color = THECOLORS["red"]
space.add(robot_body, robot_u, robot_u_leg, robot_leg, robot_l_leg)
#motor and pivot for hip
#uses measured values of angles rather than given in program
seat_motor = pymunk.SimpleMotor(seat, robot_body, 0)
seat_motor.max_force = 1e6
seat_pivot = pymunk.PivotJoint(seat, robot_body, robot_body.position + ubt)
seat_pivot._set_collide_bodies(False)
seat_pivot_lim = pymunk.RotaryLimitJoint(robot_body, seat, 0, 0.575959)
space.add(seat_motor, seat_pivot, seat_pivot_lim)
#motor and pivot for knee
max_knee_ang = 5
max_knee_ang = np.deg2rad(max_knee_ang)
knee_motor = pymunk.SimpleMotor(seat, robot_leg, 0)
knee_motor.max_force = 1e5
knee_pivot = pymunk.PivotJoint(seat, robot_leg, seat.position + (-8, 7))
knee_pivot._set_collide_bodies(False)
knee_pivot_lim = pymunk.RotaryLimitJoint(seat, robot_leg,
max_knee_ang - np.deg2rad(69),
max_knee_ang)
space.add(knee_motor, knee_pivot, knee_pivot_lim)
return seat_motor, knee_motor, robot_body, robot_leg, robot_u_leg
def __init__(self, space, base_body, body_pos, length, angle, mass=1, thickness=5):
b = body_pos
dif = (math.sin(angle) * length, math.cos(angle) * length)
v = b + dif
base_dif = body_pos - base_body.position
link_body = pymunk.Body(mass, mass * length * length / 3)
link_body.position = body_pos
self.link = pymunk.Segment(link_body, (0,0), dif, thickness)
self.link.filter = pymunk.ShapeFilter(categories=0b1, mask=0b0)
link_body.moment = pymunk.moment_for_segment(mass, self.link.a, self.link.b, thickness)
link_body.center_of_gravity = (self.link.a + self.link.b)/2.0
base_motor = pymunk.SimpleMotor(base_body, link_body, 0)
base_joint = pymunk.PinJoint(base_body, link_body, base_dif, (0,0))
base_joint.error_bias = 0.0001
base_clamp = pymunk.RotaryLimitJoint(base_body, link_body, (-3*math.pi/4), (3*math.pi/4))
space.add(self.link, link_body, base_motor, base_joint, base_clamp)
print("Body pos: " + str(link_body.position))
print("COG pos: " + str(link_body.center_of_gravity))
print("a" + str(self.link.a))
print("b" + str(self.link.b))
self.motor = base_motor
self.body = link_body
self.start_tip = v
def __init__(self, a, b, position, range=(-pi, pi), max_rate=2.0):
self.pivot = pymunk.PivotJoint(a, b, position)
self.motor = pymunk.SimpleMotor(a, b, 0)
self.motor.max_force = 1e7
self.limit = pymunk.RotaryLimitJoint(a, b, range[0], range[1])
self.limit.max_force = 1e1
self.limit.max_bias = 0.5
self.max_rate = max_rate
def create_physics(self):
constraint = pymunk.RotaryLimitJoint(
self.a.body,
self.b.body,
min=self.min,
max=self.max,
)
self._constraint = constraint
self.engine.space.add(constraint)
def __init__(self, space, pos, ul, ll, w, \
lua, lla, rua, rla):
'''Creates a passive dynamic walker. The parameters
are as follow:
space -- the pymunk space
pos -- the initial position
ul -- the length of the upper leg
ll -- the length of the lower leg
w -- the width of the robot
lua -- the angle of the left hip
lla -- the angle of the left ankle
rua -- the angle of the right hip
rla -- the angle of the right angle
'''
#print "Creating Walker: ", pos, ul, ll, w, lua, lla, rua, rla
# Constructor method
self.space = space
self.density = 1.0
self.friction = 1.0
self.elasticity = 0.4
self.group = 1
# Create left leg
self.lul = self._create_leg(pos, ul, w, lua)
ll_pos = Vec2d(0, -ul).rotated(lua) + pos
self.lll = self._create_leg(ll_pos, ll, w, lua + lla)
self.l_knee = pymunk.PivotJoint(self.lul.body, \
self.lll.body, ll_pos)
# Create right leg
self.rul = self._create_leg(pos, ul, w, rua)
rl_pos = Vec2d(0, -ul).rotated(rua) + pos
self.rll = self._create_leg(rl_pos, ll, w, rua + rla)
self.r_knee = pymunk.PivotJoint(self.rul.body, \
self.rll.body, rl_pos)
# Hip
self.hip = pymunk.PivotJoint(self.lul.body, self.rul.body, pos)
# Limit rotation on the knees between zero and pi
self.l_limit = pymunk.RotaryLimitJoint(self.lul.body, \
self.lll.body, (-pi/360.0)-lla, pi-lla)
self.r_limit = pymunk.RotaryLimitJoint(self.rul.body, \
self.rll.body, (-pi/360.0)-rla, pi-rla)
# add the constraints to the space
self.space.add(self.l_knee, self.r_knee, self.hip, \
self.l_limit, self.r_limit)
def testPickle(self):
a, b = p.Body(10, 10), p.Body(20, 20)
j = p.RotaryLimitJoint(a, b, 1, 2)
s = pickle.dumps(j)
j2 = pickle.loads(s)
self.assertEqual(j.min, j2.min)
self.assertEqual(j.max, j2.max)
self.assertEqual(j.a.mass, j2.a.mass)
self.assertEqual(j.b.mass, j2.b.mass)
def _attach_to_anchor(self):
self.joint = pymunk.PivotJoint(self.anchor.pm_body, self.pm_body, self._rel_coord_anchor, self._rel_coord_part)
self.joint.collide_bodies = False
self.limit = pymunk.RotaryLimitJoint(self.anchor.pm_body, self.pm_body,
self.angle_offset - self.rotation_range / 2,
self.angle_offset + self.rotation_range / 2)
self.motor = pymunk.SimpleMotor(self.anchor.pm_body, self.pm_body, 0)
self.pm_elements += [self.joint, self.motor, self.limit]
def __init__(self, app, pos, angle):
self.app = app
rads = angle / -57.3
hw = self.size[1] * .5
self.body = pymunk.Body()
self.body.position = pos[::2]
self.body.angle = rads
static_body = self.app.physics.space.static_body
self.collider = pymunk.Poly.create_box(self.body, self.size)
self.collider.y = pos[1]
self.collider.height = self.height
self.collider.mass = .5
self.collider.collision_type = physics.STATIC
mask = pymunk.ShapeFilter.ALL_MASKS ^ physics.STATIC_FILTER ^ physics.DOOR_FILTER
self.collider.filter = pymunk.ShapeFilter(categories=physics.DOOR_FILTER,
mask=mask)
hinge_dx = math.sin(-rads) * -hw
hinge_dy = math.cos(-rads) * -hw
self.hinge_joint = pymunk.PivotJoint(static_body, self.body,
(pos[0] + hinge_dx, pos[2] + hinge_dy))
self.rotary_spring = pymunk.DampedRotarySpring(static_body, self.body,
-rads, 5, 5)
min_angle = rads - self.swing_angle
max_angle = rads + self.swing_angle
self.rotary_limit = pymunk.RotaryLimitJoint(static_body, self.body,
min_angle, max_angle)
self.app.physics.space.add(self.body, self.collider,
self.hinge_joint, self.rotary_spring,
self.rotary_limit)
self.canvas = Canvas()
with self.canvas:
PushMatrix()
self.pos = Translate(*pos)
self.rot = Rotate(0, 0, 1, 0)
self.app.graphic_data.draw_mesh('models/door',
self.canvas,
texture='door.png')
PopMatrix()
def __init__(self,
floor,
leg_mass=5.,
leg_width=2.,
leg_length=40.,
leg_friction=.4,
init_leg_angle=30):
self.leg_length = leg_length
self.leg_mass = leg_mass
self.leg_width = leg_width
self.leg_friction = leg_friction
self.legs_min_angle = 0.
self.legs_max_angle = .9 * math.pi
self.leg1, self.joint_pos_l1 = self.create_leg(
self.leg_mass, self.leg_length, self.leg_width,
math.radians(init_leg_angle), leg_friction,
pygame.color.THECOLORS['red'])
self.leg2, self.joint_pos_l2 = self.create_leg(
self.leg_mass, self.leg_length, self.leg_width,
math.radians(-init_leg_angle), leg_friction,
pygame.color.THECOLORS['green'])
leg_position_x = floor.body.position.x / 8.
leg_position_y = floor.body.position.y
self.place_leg(self.leg1, leg_position_x, leg_position_y,
math.radians(init_leg_angle))
self.place_leg(self.leg2, leg_position_x, leg_position_y,
math.radians(-init_leg_angle))
self.rotation_center_legs_joint = pymunk.PivotJoint(
self.leg1.body, self.leg2.body, self.joint_pos_l2,
self.joint_pos_l1)
self.rotation_center_legs_joint.collide_bodies = False
self.leg1_floor_motor = pymunk.SimpleMotor(self.leg1.body, floor.body,
0.)
self.leg2_floor_motor = pymunk.SimpleMotor(self.leg2.body, floor.body,
0.)
self.legs_rotary_limit_joint = pymunk.RotaryLimitJoint(
self.leg1.body, self.leg2.body, self.legs_min_angle,
self.legs_max_angle)
def addToSpace(self, space):
# samples for door
# each sample contains information about surface point, normal and maximum radius of contacting body
self.door_samples = []
for t in np.linspace(0.1, 0.9, 5, endpoint=True):
self.door_samples.append( (Vec2d(self.x_mult * self.length/2.0 * t, -self.width/2.0), Vec2d(0,-1), float("inf")) )
radius_tab = [ 100, 50, 15, 5, 5 ]
idx = 0
for t in np.linspace(0.1, 0.9, 5, endpoint=True):
self.door_samples.append( (Vec2d(self.x_mult * self.length/2.0 * t, self.width/2.0), Vec2d(0,1), radius_tab[idx]) )
idx += 1
self.door_samples.append( (Vec2d(self.x_mult * (self.length/2.0*0.8 - 4*2.0), 4*4.0), Vec2d(0,1), float("inf")) )
self.door_samples.append( (Vec2d(self.x_mult * (self.length/2.0*0.8 - 4*2.0), 4*4.0), Vec2d(0,-1), 5) )
mass = 10.0
moment = pymunk.moment_for_poly(mass, self.fp)
self.body = pymunk.Body(mass, moment)
self.body.position = self.hinge_pos #t5
self.body.angle = self.rotation + self.init_angle
self.door_shape = pymunk.Poly(self.body, self.fp, radius=1)
self.door_shape.group = 1
# handle
self.handle_shape1 = pymunk.Segment(self.body, Vec2d(self.x_mult * (self.length/2.0*0.8), self.width*0.5), Vec2d(self.x_mult * (self.length/2.0*0.8), self.width*4.0), self.width)
self.handle_shape1.group = 1
self.handle_shape2 = pymunk.Segment(self.body, Vec2d(self.x_mult * (self.length/2.0*0.8 - self.width*4.0), self.width*4.0), Vec2d(self.x_mult * (self.length/2.0*0.8), self.width*4.0), self.width)
self.handle_shape2.group = 1
space.add(self.body, self.door_shape, self.handle_shape1, self.handle_shape2)
j = pymunk.PivotJoint(self.body, space.static_body, self.body.position)
s = pymunk.DampedRotarySpring(self.body, space.static_body, 0, 0, 90000)
space.add(j, s)
j2 = pymunk.RotaryLimitJoint(self.body, space.static_body, -(self.rotation+self.limits[1]), -(self.rotation+self.limits[0]))
space.add(j2)
def testRotaryLimitJoint(self):
a, b = p.Body(10, 10), p.Body(20, 20)
j = p.RotaryLimitJoint(a, b, 0.1, 0.2)
self.assertEqual(j.max, 0.2)
self.assertEqual(j.min, 0.1)
def __init__(self, space, screen):
self.space = space
self.screen = screen
width, height = screen.get_size()
pos = (3 * width / 4, height / 4)
shapeFilter = pm.ShapeFilter(collisionGroups["PLAYER2"],
collisionGroups["PLAYER1"])
self.coreBody = pm.Body(10, 1000)
self.coreBody.position = pos
self.rightArmAlive = True
self.leftArmAlive = True
self.rightLegAlive = True
self.leftLegAlive = True
self.torso = Body(space, screen, 2)
self.torso.shape.filter = shapeFilter
self.torsoRotationLimit = pm.RotaryLimitJoint(self.space.static_body,
self.torso.shape.body,
-math.pi / 10,
math.pi / 10)
self.torsoLocationTie = pm.PinJoint(self.coreBody,
self.torso.shape.body)
self.torsoLocationTie.distance = 0
self.torso.shape.body.position = pos
# Set up arms
self.rElbow = DefensiveBlock(self.space, self.screen)
self.rElbow.shape.body.position = self.torso.shape.body.position + (
-50, 35)
self.rElbow.shape.filter = shapeFilter
self.rUpperArm = pm.PinJoint(self.torso.shape.body,
self.rElbow.shape.body, (0, 35), (0, 0))
self.rUpperArm.distance = 40
self.rFist = OffensiveBlock(self.space, self.screen)
self.rFist.shape.filter = shapeFilter
self.rFist.shape.body.position = self.rElbow.shape.body.position + (
-25, 0)
self.rFistRotationLimit = pm.RotaryLimitJoint(self.space.static_body,
self.rFist.shape.body,
-math.pi / 5,
math.pi / 5)
self.rLowerArm = pm.PinJoint(self.rElbow.shape.body,
self.rFist.shape.body)
self.rLowerArm.distance = 50
self.lElbow = DefensiveBlock(self.space, self.screen)
self.lElbow.shape.body.position = self.torso.shape.body.position + (50,
35)
self.lElbow.shape.filter = shapeFilter
self.lUpperArm = pm.PinJoint(self.torso.shape.body,
self.lElbow.shape.body, (0, 35), (0, 0))
self.lUpperArm.distance = 40
self.lFist = OffensiveBlock(self.space, self.screen)
self.lFist.shape.filter = shapeFilter
self.lFist.shape.body.position = self.lElbow.shape.body.position + (25,
0)
self.lFistRotationLimit = pm.RotaryLimitJoint(self.space.static_body,
self.lFist.shape.body,
-math.pi / 5,
math.pi / 5)
self.lLowerArm = pm.PinJoint(self.lElbow.shape.body,
self.lFist.shape.body)
self.lLowerArm.distance = 50
# Set up legs
self.rKnee = DefensiveBlock(self.space, self.screen)
self.rKnee.shape.body.position = self.torso.shape.body.position + (
-25, -100)
self.rKnee.shape.filter = shapeFilter
self.rUpperLeg = pm.PinJoint(self.torso.shape.body,
self.rKnee.shape.body, (0, -50), (0, 0))
self.rUpperLeg.distance = 40
self.rFoot = OffensiveBlock(self.space, self.screen)
self.rFoot.shape.friction = 1.5
self.rFoot.shape.filter = shapeFilter
self.rFoot.shape.body.position = self.rKnee.shape.body.position + (-25,
-10)
self.rFootRotationLimit = pm.RotaryLimitJoint(self.space.static_body,
self.rFoot.shape.body,
-math.pi / 5,
math.pi / 5)
self.rLowerLeg = pm.PinJoint(self.rKnee.shape.body,
self.rFoot.shape.body, (0, 0), (0, 25))
self.rLowerLeg.distance = 50
self.lKnee = DefensiveBlock(self.space, self.screen)
self.lKnee.shape.body.position = self.torso.shape.body.position + (
25, -100)
self.lKnee.shape.filter = shapeFilter
self.lUpperLeg = pm.PinJoint(self.torso.shape.body,
self.lKnee.shape.body, (0, -50), (0, 0))
self.lUpperLeg.distance = 40
self.rLowerLeg = pm.PinJoint(self.rKnee.shape.body,
self.rFoot.shape.body, (0, 0), (0, 25))
self.rLowerLeg.distance = 50
self.lKnee = DefensiveBlock(self.space, self.screen)
self.lKnee.shape.body.position = self.torso.shape.body.position + (
25, -100)
self.lKnee.shape.filter = shapeFilter
self.lUpperLeg = pm.PinJoint(self.torso.shape.body,
self.lKnee.shape.body, (0, -50), (0, 0))
self.lUpperLeg.distance = 40
self.lFoot = OffensiveBlock(self.space, self.screen)
self.lFoot.shape.friction = 1.5
self.lFoot.shape.filter = shapeFilter
self.lFoot.shape.body.position = self.lKnee.shape.body.position + (25,
-10)
self.lFootRotationLimit = pm.RotaryLimitJoint(self.space.static_body,
self.lFoot.shape.body,
-math.pi / 5,
math.pi / 5)
self.lLowerLeg = pm.PinJoint(self.lKnee.shape.body,
self.lFoot.shape.body, (0, 0), (0, 25))
self.lLowerLeg.distance = 50
self.rLegDownwardForce = pm.DampedSpring(self.coreBody,
self.rFoot.shape.body, (0, 0),
(0, 25), 300, 1250, 1)
self.lLegDownwardForce = pm.DampedSpring(self.coreBody,
self.lFoot.shape.body, (0, 0),
(0, 25), 300, 1250, 1)
self.space.add(self.coreBody, self.torsoRotationLimit,
self.torsoLocationTie, self.rUpperLeg,
self.rLegDownwardForce, self.lUpperLeg,
self.lLegDownwardForce, self.rFootRotationLimit,
self.lFootRotationLimit, self.rFistRotationLimit,
self.lFistRotationLimit, self.lLowerLeg, self.rLowerLeg,
self.rUpperArm, self.rLowerArm, self.lUpperArm,
self.lLowerArm)
def run_once(self, keep=False, start=False, verbose=False, **kwargs):
pygame.init()
if not self.headless:
self.screen = pygame.display.set_mode(self.display_size, self.display_flags)
width, height = self.screen.get_size()
self.draw_options = pymunk.pygame_util.DrawOptions(self.screen)
def to_pygame(p):
"""Small hack to convert pymunk to pygame coordinates"""
return int(p.x), int(-p.y+height)
def from_pygame(p):
return to_pygame(p)
clock = pygame.time.Clock()
running = True
font = pygame.font.Font(None, 16)
# Create the torso box.
box_width = 50
box_height = 100
# box_width = 75
# box_height = 200
# box_width = 200
# box_height = 100
leg_length = 100
# leg_length = 125
leg_thickness = 2
leg_shape_filter = pymunk.ShapeFilter(group=LEG_GROUP)
# Create torso.
torso_mass = 500
torso_points = [(-box_width/2, -box_height/2), (-box_width/2, box_height/2), (box_width/2, box_height/2), (box_width/2, -box_height/2)]
moment = pymunk.moment_for_poly(torso_mass, torso_points)
body1 = pymunk.Body(torso_mass, moment)
body1.position = (self.display_size[0]/2, self.ground_y+box_height/2+leg_length)
body1.start_position = Vec2d(body1.position)
body1.start_angle = body1.angle
body1.center_of_gravity = (0, 0) # centered
# body1.center_of_gravity = (0, box_height/2) # top-heavy
# body1.center_of_gravity = (box_width/2, 0) # right-heavy
shape1 = pymunk.Poly(body1, torso_points)
shape1.filter = leg_shape_filter
shape1.friction = 0.8
shape1.elasticity = 0.0
self.space.add(body1, shape1)
# Create leg extending from the right to the origin.
leg_mass = 1
leg_points = [
(leg_thickness/2, -leg_length/2),
(-leg_thickness/2, -leg_length/2),
# (-leg_thickness/2, leg_length/2-leg_thickness),
# (leg_thickness/2, leg_length/2-leg_thickness/2)
(-leg_thickness/2, leg_length/2),
(leg_thickness/2, leg_length/2)
]
leg_moment = pymunk.moment_for_poly(leg_mass, leg_points)
body2 = pymunk.Body(leg_mass, leg_moment)
if self.leg_position == CORNER:
# Position leg at far corner of torso.
body2.position = (self.display_size[0]/2-box_width/2+leg_thickness/2, self.ground_y+leg_length/2)
elif self.leg_position == OFFSET:
# Position leg near center of torso, but still offset to create slight instability.
body2.position = (self.display_size[0]/2-leg_thickness/2, self.ground_y+leg_length/2)
elif self.leg_position == CENTER:
body2.position = (self.display_size[0]/2, self.ground_y+leg_length/2)
else:
raise NotImplementedError
body2.start_position = Vec2d(body2.position)
body2.start_angle = body2.angle
shape2 = pymunk.Poly(body2, leg_points)
shape2.filter = leg_shape_filter
shape2.friction = 0.8
shape2.elasticity = 0.0
self.space.add(body2, shape2)
# Link bars together at end.
# pj = pymunk.PivotJoint(body1, body2, (self.display_size[0]/2-box_width/2, self.ground_y+leg_length-leg_thickness))
if self.leg_position == CORNER:
pj = pymunk.PivotJoint(body1, body2, (self.display_size[0]/2-box_width/2+leg_thickness/2, self.ground_y+leg_length)) # far corner
elif self.leg_position == OFFSET:
pj = pymunk.PivotJoint(body1, body2, (self.display_size[0]/2-leg_thickness/2, self.ground_y+leg_length)) # near center
elif self.leg_position == CENTER:
pj = pymunk.PivotJoint(body1, body2, (self.display_size[0]/2, self.ground_y+leg_length)) # near center
else:
raise NotImplementedError
# pj = pymunk.PivotJoint(body1, body2, Vec2d(body2.position))
self.space.add(pj)
# Attach the foot to the ground in a fixed position.
# We raise it above by the thickness of the leg to simulate a ball-foot. Otherwise, the default box foot creates discontinuities.
if self.leg_position == CORNER:
pj = pymunk.PivotJoint(self.space.static_body, body2, (self.display_size[0]/2-box_width/2+leg_thickness/2, self.ground_y+leg_thickness)) # far
elif self.leg_position == OFFSET:
pj = pymunk.PivotJoint(self.space.static_body, body2, (self.display_size[0]/2-leg_thickness/2, self.ground_y+leg_thickness)) # off center
elif self.leg_position == CENTER:
pj = pymunk.PivotJoint(self.space.static_body, body2, (self.display_size[0]/2, self.ground_y+leg_thickness)) # center
else:
raise NotImplementedError
self.space.add(pj)
# Actuate the bars via a motor.
motor_joint = None
motor_joint = pymunk.SimpleMotor(body1, body2, 0)
# motor_joint.max_force = 1e10 # mimicks default infinity, too strong, breaks rotary limit joints
motor_joint.max_force = 1e9
# motor_joint.max_force = 1e7 # too weak, almost no movement
self.space.add(motor_joint)
# Simulate friction on the foot joint.
# https://chipmunk-physics.net/forum/viewtopic.php?f=1&t=2916
foot_gj = pymunk.GearJoint(self.space.static_body, body2, 0.0, 1.0)
foot_gj.max_force = 1e8
self.space.add(foot_gj)
# Simulate friction on the hip joint.
# https://chipmunk-physics.net/forum/viewtopic.php?f=1&t=2916
# hip_gj = pymunk.GearJoint(body1, body2, 0.0, 1.0)
# hip_gj.max_force = 1e10
# self.space.add(hip_gj)
# Add hard stops to leg pivot so the leg can't rotate through the torso.
angle_range = pi/4. # 45 deg
# angle_range = pi/2. # 90 deg
hip_limit_joint = pymunk.RotaryLimitJoint(body1, body2, -angle_range, angle_range) # -45deg:+45deg
self.space.add(hip_limit_joint)
# default angle = 270/2 = 135 deg
servo = None
if motor_joint:
servo = ServoController(motor_joint, max_rate=20.0)
servo.set_degrees(135)
# pygame.time.set_timer(USEREVENT+1, 70000) # apply force
# pygame.time.set_timer(USEREVENT+2, 120000) # reset
# pygame.event.post(pygame.event.Event(USEREVENT+1))
# pygame.mouse.set_visible(False)
self.reset()
last_body1_pos = None
last_body1_vel = None
simulate = False
self.cnt = 0
failed = None
simulate = start
current_state = None
# The point at which the torso has toppled over.
failure_degrees = 45
while running:
self.cnt += 1
# print('angles:', body1.angle, body2.angle)
# print('torso force:', body1.force)
# print('body1.position: %.02f %.02f' % (body1.position.x, body1.position.y))
current_body1_vel = None
if last_body1_pos:
current_body1_vel = body1.position - last_body1_pos
# print('current_body1_vel: %.02f %.02f' % (current_body1_vel.x, current_body1_vel.y))
current_body1_accel = None
if last_body1_vel:
current_body1_accel = current_body1_vel - last_body1_vel
# print('current_body1_accel: %.02f %.02f' % (current_body1_accel.x, current_body1_accel.y))
# Angle of the torso, where 0 degrees is the torso perfectly parallel to the ground.
# A positive angle is clockwise rotation.
# This simulates the IMU's y euler angle measurement.
torso_angle = -body1.angle * 180/pi
# print('torso_angle:', torso_angle)
# Angle of the hip servo.
# A positive angle is clockwise rotation.
# This simulates the estimated hip servo potentiometer.
hip_angle = (body2.angle - body1.angle) * 180/pi # 0 degrees means leg is angled straight down
# Angle of the foot, where 0 degrees is the foot perfectly perpedicular to the ground.
# A positive angle is clockwise rotation.
foot_angle = -body2.angle * 180/pi
# print('foot_angle:', foot_angle)
# Re-frame angle so that 135 degrees indicates the straight down orientation.
hip_angle += 135
# print('hip_angle:', hip_angle)
current_state = [torso_angle/360., hip_angle/360., foot_angle/360.]
# assert not [_ for _ in current_state if abs(_) > 1]
# Auto-shift COG using foot angle as proportion of shift
# iters=163
# If we lean right, shift COG left, and vice versa.
# body1.center_of_gravity = (-box_width/2*foot_angle/90.*10, 0)
# cog_limit = box_width/2
# print('cog_limit:', cog_limit)
# print('foot_angle:', foot_angle)
# dist_from_center = sin(-body2.angle)*leg_length
# print('dist_from_center:', dist_from_center)
# max_dist_from_center = sin(pi/4)*leg_length
# print('max_dist_from_center:', max_dist_from_center)
# comp_rate = -dist_from_center/max_dist_from_center
# print('comp_rate:', comp_rate)
# gamma = 4
# comp_rate2 = exp(abs(comp_rate*gamma))
# if comp_rate < 0:
# comp_rate2 = -comp_rate2
# print('comp_rate2:', comp_rate2)
# comp_rate3 = max(min(comp_rate2, cog_limit), -cog_limit)
# print('comp_rate3:', comp_rate3)
# body1.center_of_gravity = (comp_rate3, 0)
if self.verbose:
print('foot.angular_velocity:', body2.angular_velocity) # positive=CCW, negative=CW
if isnan(float(body2.angular_velocity)):
print('Physics breakdown! Terminating simulation!', file=sys.stderr)
return
# Auto-shift COG using foot angular velocity direction.
# iters=403
servo.set_position(1500)
servo.attach()
# if body2.angular_velocity > 0:
# # falling left
# body1.center_of_gravity = (box_width/2, 0) # shift weight right
# else:
# # falling right
# body1.center_of_gravity = (-box_width/2, 0) # shift weight left
# Auto-shift COG using foot angular velocity magnitude.
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*1, 0)# iters=158
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*2, 0)# iters=161
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*2.5, 0)# iters=157
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*3, 0)# iters=160
# body1.center_of_gravity = (box_width/2*body2.angular_velocity/5*5, 0)# iters=153
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*1, box_width/2), -box_width/2), 0)# iters=153
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*2, box_width/2), -box_width/2), 0)# iters=135
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*3, box_width/2), -box_width/2), 0)# iters=132
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*100, box_width/2), -box_width/2), 0)# iters=290
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*50, box_width/2), -box_width/2), 0)# iters=216
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*25, box_width/2), -box_width/2), 0)# iters=278
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*200, box_width/2), -box_width/2), 0)# iters=293
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*1000, box_width/2), -box_width/2), 0)# iters=304
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*10000, box_width/2), -box_width/2), 0)# iters=333
# body1.center_of_gravity = (max(min(box_width/2*body2.angular_velocity*100000, box_width/2), -box_width/2), 0)# iters=333
if self.verbose:
print('cog:', body1.center_of_gravity)
# Auto-shift hip angle to level.
#TODO
# Handle balancer agent input.
if self.balancer:
relative_position_change = self.balancer.get_action(state=current_state, actions=self.get_actions(self.balancer))
if self.verbose:
print('balancer action:', relative_position_change)
# Otherwise convert the relative +/- position into an absolute servo position.
hip_position = servo.degree_to_position(hip_angle)
new_hip_position = hip_position + relative_position_change
servo.set_position(new_hip_position)
# servo.set_position(1000)#manual
servo.attach()
# Handle shifter agent input.
if self.shifter:
shifter_state = [body2.angular_velocity/10.]
cog_shift = self.shifter.get_action(
state=list(shifter_state),
actions=[-box_width/2., -box_width/4., -box_width/16., -box_width/32., 0, +box_width/2., +box_width/4., +box_width/16., +box_width/32.])
body1.center_of_gravity = (max(min(cog_shift, box_width/2), -box_width/2), 0)
# Handle user input.
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYDOWN and event.key in (K_q, K_ESCAPE)):
running = False
elif event.type == KEYDOWN and event.key == K_s:
# Start/stop simulation.
simulate = not simulate
elif event.type == KEYDOWN and event.key == K_r:
# Reset.
self.reset_bodies()
elif event.type == KEYDOWN and event.key == K_LEFT:
# Angle backwards by rotating torso about the hips counter-clockwise.
servo.set_degrees(servo.min_degrees)
servo.attach()
elif event.type == KEYDOWN and event.key == K_RIGHT:
# Angle forwarads by rotating torso about the hips clockwise.
servo.set_degrees(servo.max_degrees)
servo.attach()
elif event.type == KEYUP:
# Otherwise, make the servo go unpowered and idle.
servo.detach()
servo.update(current_degrees=hip_angle)
if not self.headless:
self.draw()
last_body1_pos = Vec2d(body1.position)
if current_body1_vel:
last_body1_vel = Vec2d(current_body1_vel)
### Update physics
fps = 50
iterations = 25
dt = 1.0/float(fps)/float(iterations)
if simulate:
for x in range(iterations): # 10 iterations to get a more stable simulation
self.space.step(dt)
if not self.headless:
pygame.display.flip()
clock.tick(fps)
# Give balancer feedback.
# The balancer's objective is to keep the torso as level as possible, balanced on the foot.
failed = abs(foot_angle) > failure_degrees
if self.balancer:
# Feedback is the inverse distance of the torso's angle from center.
if failed:
# Terminal failure.
feedback = -1
else:
# Otherwise, grade performance based on how level the torso is.
feedback = valmap(abs(torso_angle), 0, 90, 1, -1)
# print('failed:', failed)
self.balancer.reinforce(feedback=feedback+self.cnt, state=current_state, end=failed)
if self.shifter:
if failed:
feedback = -1
else:
# feedback = 0
feedback = (90. - abs(foot_angle))/90.
self.shifter.reinforce(feedback=feedback, state=shifter_state, end=failed)
# Check failure criteria.
if not keep and (failed or self.cnt >= 1000):
break
print('Result: %s' % {True: 'failure', False:'aborted', None:'aborted'}[failed])
print('Iterations: %i' % self.cnt)
if self.balancer:
self.balancer.save()
if self.shifter:
self.shifter.save()
def __init__(self):
left_thigh = pymunk.Body(body_type=pymunk.Body.DYNAMIC)
left_thigh.position = (100, 175)
left_thigh_segment = pymunk.Segment(left_thigh, (0, 25), (0, -25), 2)
left_thigh_segment.density = 1
left_thigh_segment.friction = 0.46
left_thigh_segment.filter = pymunk.ShapeFilter(group=0b1)
left_knee_position = (100, 150)
left_leg = pymunk.Body(body_type=pymunk.Body.DYNAMIC)
left_leg.position = (100, 125)
left_leg_segment = pymunk.Segment(left_leg, (0, 25), (0, -25), 2)
left_leg_segment.density = 1
left_leg_segment.friction = 0.8
left_leg_segment.filter = pymunk.ShapeFilter(group=0b1)
left_knee_joint = pymunk.PivotJoint(left_thigh, left_leg,
left_knee_position)
left_knee_limit = pymunk.RotaryLimitJoint(left_thigh, left_leg,
-math.pi / 2, 0)
left_ankle_position = (100, 100)
left_foot = pymunk.Body(body_type=pymunk.Body.DYNAMIC)
left_foot.position = (110, 100)
left_foot_segment = pymunk.Segment(left_foot, (-10, 0), (10, 0), 2)
left_foot_segment.density = 1
left_foot_segment.friction = 0.8
left_foot_segment.filter = pymunk.ShapeFilter(group=0b1)
left_ankle_joint = pymunk.PivotJoint(left_leg, left_foot,
left_ankle_position)
left_ankle_limit = pymunk.RotaryLimitJoint(left_leg, left_foot,
-math.pi / 6, math.pi / 6)
# right_leg
right_thigh = pymunk.Body(body_type=pymunk.Body.DYNAMIC)
right_thigh.position = (100, 175)
right_thigh_segment = pymunk.Segment(right_thigh, (0, 25), (0, -25), 2)
right_thigh_segment.density = 1
right_thigh_segment.friction = 0.46
right_thigh_segment.filter = pymunk.ShapeFilter(group=0b1)
right_knee_position = (100, 150)
right_leg = pymunk.Body(body_type=pymunk.Body.DYNAMIC)
right_leg.position = (100, 125)
right_leg_segment = pymunk.Segment(right_leg, (0, 25), (0, -25), 2)
right_leg_segment.density = 1
right_leg_segment.friction = 0.8
right_leg_segment.filter = pymunk.ShapeFilter(group=0b1)
right_knee_joint = pymunk.PivotJoint(right_thigh, right_leg,
right_knee_position)
right_knee_limit = pymunk.RotaryLimitJoint(right_thigh, right_leg,
-math.pi / 2, 0)
right_ankle_position = (100, 100)
right_foot = pymunk.Body(body_type=pymunk.Body.DYNAMIC)
right_foot.position = (110, 100)
right_foot_segment = pymunk.Segment(right_foot, (-10, 0), (10, 0), 2)
right_foot_segment.density = 1
right_foot_segment.friction = 0.8
right_foot_segment.filter = pymunk.ShapeFilter(group=0b1)
right_ankle_joint = pymunk.PivotJoint(right_leg, right_foot,
right_ankle_position)
right_ankle_limit = pymunk.RotaryLimitJoint(right_leg, right_foot,
-math.pi / 6, math.pi / 6)
hip_position = (100, 200)
torso = pymunk.Body(body_type=pymunk.Body.DYNAMIC)
torso.position = (100, 240)
torso_segment = pymunk.Segment(torso, (0, 40), (0, -40), 2)
torso_segment.density = 1
torso_segment.friction = 0.46
torso_segment.filter = pymunk.ShapeFilter(group=0b1)
left_hip_joint = pymunk.PivotJoint(torso, left_thigh, hip_position)
left_hip_limit = pymunk.RotaryLimitJoint(torso, left_thigh,
-math.pi / 2, math.pi / 2)
right_hip_joint = pymunk.PivotJoint(torso, right_thigh, hip_position)
right_hip_limit = pymunk.RotaryLimitJoint(torso, right_thigh,
-math.pi / 2, math.pi / 2)
space.add(left_thigh, left_thigh_segment, left_leg, left_leg_segment,
left_foot, left_foot_segment)
space.add(right_thigh, right_thigh_segment, right_leg,
right_leg_segment, right_foot, right_foot_segment)
space.add(torso, torso_segment, left_hip_joint, right_hip_joint,
left_knee_joint, right_knee_joint, left_ankle_joint,
right_ankle_joint)
space.add(left_hip_limit, right_hip_limit, left_knee_limit,
right_knee_limit, left_ankle_limit, right_ankle_limit)
self.l_thigh = left_thigh
self.l_thigh_seg = left_thigh_segment
self.l_leg = left_leg
self.l_leg_seg = left_leg_segment
self.l_foot = left_foot
self.l_foot_seg = left_foot_segment
self.r_thigh = right_thigh
self.r_thigh_seg = right_thigh_segment
self.r_leg = right_leg
self.r_leg_seg = right_leg_segment
self.r_foot = right_foot
self.r_foot_seg = right_foot_segment
self.chest = torso
self.chest_seg = torso_segment
self.alive = True
def load_elements(self):
self.add_element(self.position, (22, 20), 60.0,
pg.Rect(58, 32, 30, 25))
self.add_element(self.position + pm.Vec2d(0, -14), (12, 12), 4.0,
pg.Rect(58, 106, 12, 12))
self.add_element(self.position + pm.Vec2d(-11, -8), (11, 7), 2.0,
pg.Rect(85, 20, 11, 7))
self.add_element(self.position + pm.Vec2d(-19, -8), (14, 8), 2.0,
pg.Rect(71, 19, 14, 8))
self.add_element(self.position + pm.Vec2d(-6, 14), (11, 12), 8.0,
pg.Rect(181, 64, 11, 12))
self.add_element(self.position + pm.Vec2d(-5, 25), (11, 12), 4.0,
pg.Rect(181, 76, 11, 12))
self.add_element(self.position + pm.Vec2d(11, -8), (11, 7), 2.0,
pg.Rect(160, 20, 11, 7))
self.add_element(self.position + pm.Vec2d(19, -8), (14, 8), 2.0,
pg.Rect(171, 19, 14, 8))
self.add_element(self.position + pm.Vec2d(6, 14), (11, 12), 8.0,
pg.Rect(64, 64, 11, 12))
self.add_element(self.position + pm.Vec2d(5, 25), (11, 12), 4.0,
pg.Rect(64, 76, 11, 12))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[1][0],
self.position + pm.Vec2d(0, -11)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[1][0],
self._elements[0][0].position + pm.Vec2d(0, -15)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[2][0],
self.position + pm.Vec2d(-10, -7)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[0][0], self._elements[2][0],
math.radians(-90), math.radians(90)))
self.add_joint(
pm.PivotJoint(self._elements[2][0], self._elements[3][0],
self.position + pm.Vec2d(-16, -11)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[2][0], self._elements[3][0],
math.radians(0), math.radians(110)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[4][0],
self.position + pm.Vec2d(-5, 10)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[0][0], self._elements[4][0],
math.radians(0), math.radians(90)))
self.add_joint(
pm.PivotJoint(self._elements[4][0], self._elements[5][0],
self.position + pm.Vec2d(-4, 18)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[4][0], self._elements[5][0],
math.radians(-110), math.radians(0)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[6][0],
self.position + pm.Vec2d(10, -7)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[0][0], self._elements[6][0],
math.radians(-90), math.radians(90)))
self.add_joint(
pm.PivotJoint(self._elements[6][0], self._elements[7][0],
self.position + pm.Vec2d(16, -8)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[6][0], self._elements[7][0],
math.radians(-110), math.radians(0)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[8][0],
self.position + pm.Vec2d(5, 10)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[0][0], self._elements[8][0],
math.radians(-90), math.radians(0)))
self.add_joint(
pm.PivotJoint(self._elements[8][0], self._elements[9][0],
self.position + pm.Vec2d(4, 18)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[8][0], self._elements[9][0],
math.radians(0), math.radians(110)))
def __init__(self, _id, _parent, _name):
super(SceneLevel, self).__init__(_id, _parent)
self.name = _name
level_data_path = os.path.join(SceneLevel.LEVEL_ROOT, self.name)
config_path = os.path.join(level_data_path, SceneLevel.CONFIG_FILE)
config_file_handler = open(config_path, "r")
config = json.load(config_file_handler)
config_file_handler.close()
self.fps = config["FPS"]
# pymunk space
self.space = pymunk.Space()
self.space.gravity = config["GRAVITY"]
self.space.damping = config["DAMPING"]
# static data (layout of terrain, etc.)
map_path = os.path.join(level_data_path, SceneLevel.MAP_BODY_FILE)
map_file_handler = open(map_path, "r")
self.map_data = json.load(map_file_handler)
map_file_handler.close()
map_img_path = os.path.join(level_data_path, SceneLevel.MAP_IMG_FILE)
self.map_img = pygame.image.load(map_img_path)
body = None
for rev_shape in self.map_data:
if len(rev_shape) <= 2:
continue
print(rev_shape)
pos = get_center(rev_shape)
shape = [(x[1] - pos[1], x[0] - pos[0]) for x in rev_shape]
pos = tuple(reversed(pos))
body = pymunk.Body(body_type=pymunk.Body.STATIC)
body.position = pos
shape = pymunk.Poly(body, shape, radius=1)
shape.friction = config["FRICTION"] / 2
self.space.add(body, shape)
chara_img_path = os.path.join("pic", "CHARA.png")
self.chara_img = pygame.image.load(chara_img_path)
chara_shape = config["CHARA_BODY"]
chara_shape_loc = get_center(chara_shape)
chara_shape = [(x[1] - chara_shape_loc[1], x[0] - chara_shape_loc[0])
for x in chara_shape]
chara_shape_loc = tuple(reversed(chara_shape_loc))
self.chara_body = pymunk.Body(config["CHARA_MASS"],
config["CHARA_MOVEMENT"])
self.chara_body.position = chara_shape_loc
chara_shape = pymunk.Poly(self.chara_body, chara_shape, radius=1)
chara_shape.friction = config["FRICTION"] / 2
self.space.add(chara_shape, self.chara_body)
self.chara_offset = tuple(reversed(config["CHARA_OFFSET"]))
self.chara_ori_right = 1
def limit_velocity(body, gravity, damping, dt):
max_velocity = 200
pymunk.Body.update_velocity(body, gravity, damping, dt)
l = body.velocity.length
if l > max_velocity:
body.velocity = body.velocity * (max_velocity / l)
self.chara_body.velocity_func = limit_velocity
c = pymunk.RotaryLimitJoint(body, self.chara_body, 0, 0)
self.space.add(c)
self.pressing_key = None
def testMax(self):
a, b = p.Body(10, 10), p.Body(20, 20)
j = p.RotaryLimitJoint(a, b, 0, 1)
self.assertEqual(j.max, 1)
j.max = 2
self.assertEqual(j.max, 2)
def setup(self, *args, **kwargs):
super().setup(*args, **kwargs)
# physics setup, starting with main body
# signature: moment_for_circle(mass, inner_rad, outer_rad, offset)
inertia = pm.moment_for_circle(self.mass, 0, self.radius, (0, 0))
self.robot_body = body = pm.Body(self.mass, inertia)
body.position = self.init_pos
body.angle = self.init_angle
self.add_to_space(body)
# For control. The rough joint setup was taken form tank.py in the
# pymunk examples.
self.control_body = control_body = pm.Body(body_type=pm.Body.KINEMATIC)
control_body.position = self.init_pos
control_body.angle = self.init_angle
self.add_to_space(control_body)
pos_control_joint = pm.PivotJoint(control_body, body, (0, 0), (0, 0))
pos_control_joint.max_bias = 0
pos_control_joint.max_force = self.phys_vars.robot_pos_joint_max_force
self.add_to_space(pos_control_joint)
rot_control_joint = pm.GearJoint(control_body, body, 0.0, 1.0)
rot_control_joint.error_bias = 0.0
rot_control_joint.max_bias = 2.5
rot_control_joint.max_force = self.phys_vars.robot_rot_joint_max_force
self.add_to_space(rot_control_joint)
# googly eye control bodies & joints
self.pupil_bodies = []
for eye_side in [-1, 1]:
eye_mass = self.mass / 10
eye_radius = self.radius
eye_inertia = pm.moment_for_circle(eye_mass, 0, eye_radius, (0, 0))
eye_body = pm.Body(eye_mass, eye_inertia)
eye_body.angle = self.init_angle
eye_joint = pm.DampedRotarySpring(body, eye_body, 0, 0.1, 3e-3)
eye_joint.max_bias = 3.0
eye_joint.max_force = 0.001
self.pupil_bodies.append(eye_body)
self.add_to_space(eye_body, eye_joint)
# finger bodies/controls (annoying)
finger_thickness = 0.25 * self.radius
finger_upper_length = 1.1 * self.radius
finger_lower_length = 0.7 * self.radius
self.finger_bodies = []
self.finger_motors = []
finger_vertices = []
finger_inner_vertices = []
self.target_finger_angle = 0.0
for finger_side in [-1, 1]:
# basic finger body
finger_verts = make_finger_vertices(
upper_arm_len=finger_upper_length,
forearm_len=finger_lower_length,
thickness=finger_thickness,
side_sign=finger_side)
finger_vertices.append(finger_verts)
finger_inner_verts = make_finger_vertices(
upper_arm_len=finger_upper_length - ROBOT_LINE_THICKNESS * 2,
forearm_len=finger_lower_length - ROBOT_LINE_THICKNESS * 2,
thickness=finger_thickness - ROBOT_LINE_THICKNESS * 2,
side_sign=finger_side)
finger_inner_verts = [[(x, y + ROBOT_LINE_THICKNESS)
for x, y in box]
for box in finger_inner_verts]
finger_inner_vertices.append(finger_inner_verts)
# these are movement limits; they are useful below, but also
# necessary to make initial positioning work
if finger_side < 0:
lower_rot_lim = -self.finger_rot_limit_inner
upper_rot_lim = self.finger_rot_limit_outer
if finger_side > 0:
lower_rot_lim = -self.finger_rot_limit_outer
upper_rot_lim = self.finger_rot_limit_inner
finger_mass = self.mass / 8
finger_inertia = pm.moment_for_poly(finger_mass,
sum(finger_verts, []))
finger_body = pm.Body(finger_mass, finger_inertia)
if finger_side < 0:
delta_finger_angle = upper_rot_lim
finger_body.angle = self.init_angle + delta_finger_angle
else:
delta_finger_angle = lower_rot_lim
finger_body.angle = self.init_angle + delta_finger_angle
# position of finger relative to body
finger_rel_pos = (finger_side * self.radius * 0.45,
self.radius * 0.1)
finger_rel_pos_rot = gtools.rotate_vec(finger_rel_pos,
self.init_angle)
finger_body.position = gtools.add_vecs(body.position,
finger_rel_pos_rot)
self.add_to_space(finger_body)
self.finger_bodies.append(finger_body)
# pin joint to keep it in place (it will rotate around this point)
finger_pin = pm.PinJoint(
body,
finger_body,
finger_rel_pos,
(0, 0),
)
finger_pin.error_bias = 0.0
self.add_to_space(finger_pin)
# rotary limit joint to stop it from getting too far out of line
finger_limit = pm.RotaryLimitJoint(body, finger_body,
lower_rot_lim, upper_rot_lim)
finger_limit.error_bias = 0.0
self.add_to_space(finger_limit)
# motor to move the fingers around (very limited in power so as not
# to conflict with rotary limit joint)
finger_motor = pm.SimpleMotor(body, finger_body, 0.0)
finger_motor.rate = 0.0
finger_motor.max_bias = 0.0
finger_motor.max_force = self.phys_vars.robot_finger_max_force
self.add_to_space(finger_motor)
self.finger_motors.append(finger_motor)
# For collision. Main body circle. Signature: Circle(body, radius,
# offset).
robot_group = 1
body_shape = pm.Circle(body, self.radius, (0, 0))
body_shape.filter = pm.ShapeFilter(group=robot_group)
body_shape.friction = 0.5
self.add_to_space(body_shape)
# the fingers
finger_shapes = []
for finger_body, finger_verts, finger_side in zip(
self.finger_bodies, finger_vertices, [-1, 1]):
finger_subshapes = []
for finger_subverts in finger_verts:
finger_subshape = pm.Poly(finger_body, finger_subverts)
finger_subshape.filter = pm.ShapeFilter(group=robot_group)
# grippy fingers
finger_subshape.friction = 5.0
finger_subshapes.append(finger_subshape)
self.add_to_space(*finger_subshapes)
finger_shapes.append(finger_subshapes)
# graphics setup
# draw a circular body
circ_body_in = r.make_circle(radius=self.radius - ROBOT_LINE_THICKNESS,
res=100)
circ_body_out = r.make_circle(radius=self.radius, res=100)
robot_colour = COLOURS_RGB['grey']
dark_robot_colour = darken_rgb(robot_colour)
light_robot_colour = lighten_rgb(robot_colour, 4)
circ_body_in.set_color(*robot_colour)
circ_body_out.set_color(*dark_robot_colour)
# draw the two fingers
self.finger_xforms = []
finger_outer_geoms = []
finger_inner_geoms = []
for finger_outer_subshapes, finger_inner_verts, finger_side in zip(
finger_shapes, finger_inner_vertices, [-1, 1]):
finger_xform = r.Transform()
self.finger_xforms.append(finger_xform)
for finger_subshape in finger_outer_subshapes:
vertices = [(v.x, v.y) for v in finger_subshape.get_vertices()]
finger_outer_geom = r.make_polygon(vertices)
finger_outer_geom.set_color(*robot_colour)
finger_outer_geom.add_attr(finger_xform)
finger_outer_geoms.append(finger_outer_geom)
for vertices in finger_inner_verts:
finger_inner_geom = r.make_polygon(vertices)
finger_inner_geom.set_color(*light_robot_colour)
finger_inner_geom.add_attr(finger_xform)
finger_inner_geoms.append(finger_inner_geom)
for geom in finger_outer_geoms:
self.viewer.add_geom(geom)
for geom in finger_inner_geoms:
self.viewer.add_geom(geom)
# draw some cute eyes
eye_shapes = []
self.pupil_transforms = []
for x_sign in [-1, 1]:
eye = r.make_circle(radius=0.2 * self.radius, res=20)
eye.set_color(1.0, 1.0, 1.0)
eye_base_transform = r.Transform().set_translation(
x_sign * 0.4 * self.radius, 0.3 * self.radius)
eye.add_attr(eye_base_transform)
pupil = r.make_circle(radius=0.12 * self.radius, res=10)
pupil.set_color(0.1, 0.1, 0.1)
# The pupils point forward slightly
pupil_transform = r.Transform()
pupil.add_attr(r.Transform().set_translation(
0, self.radius * 0.07))
pupil.add_attr(pupil_transform)
pupil.add_attr(eye_base_transform)
self.pupil_transforms.append(pupil_transform)
eye_shapes.extend([eye, pupil])
# join them together
self.robot_xform = r.Transform()
robot_compound = r.Compound([circ_body_out, circ_body_in, *eye_shapes])
robot_compound.add_attr(self.robot_xform)
self.viewer.add_geom(robot_compound)
def testMin(self):
a, b = p.Body(10, 10), p.Body(20, 20)
j = p.RotaryLimitJoint(a, b, 1, 0)
self.assertEqual(j.min, 1)
j.min = 2
self.assertEqual(j.min, 2)
def addToSpace(self, space):
line_width = 4.0
p1 = Vec2d(-self.width / 2.0, self.depth / 2.0)
p2 = Vec2d(-self.width / 2.0, -self.depth / 2.0)
p3 = Vec2d(self.width / 2.0, -self.depth / 2.0)
p4 = Vec2d(self.width / 2.0, self.depth / 2.0)
p5 = Vec2d(-self.width / 2.0,
self.depth / 2.0 + line_width * 2) # left door joint
t1 = p1.rotated(
self.rotation
) + self.position #transform(p1, self.position, self.rotation)
t2 = p2.rotated(
self.rotation
) + self.position #transform(p2, self.position, self.rotation)
t3 = p3.rotated(
self.rotation
) + self.position #transform(p3, self.position, self.rotation)
t4 = p4.rotated(
self.rotation
) + self.position #transform(p4, self.position, self.rotation)
t5 = p5.rotated(
self.rotation
) + self.position #transform(p5, self.position, self.rotation)
static = [
pymunk.Segment(space.static_body, t1, t2, line_width),
pymunk.Segment(space.static_body, t2, t3, line_width),
pymunk.Segment(space.static_body, t3, t4, line_width)
]
for s in static:
s.friction = 1.
s.group = 1
space.add(static)
# samples for door
# each sample contains information about surface point, normal and maximum radius of contacting body
self.door_samples = []
for t in np.linspace(0.1, 0.9, 5, endpoint=True):
self.door_samples.append(
(Vec2d(self.width / 2.0 * t,
-line_width / 2.0), Vec2d(0, -1), float("inf")))
radius_tab = [100, 50, 15, 5, 5]
idx = 0
for t in np.linspace(0.1, 0.9, 5, endpoint=True):
self.door_samples.append(
(Vec2d(self.width / 2.0 * t,
line_width / 2.0), Vec2d(0, 1), radius_tab[idx]))
idx += 1
self.door_samples.append((Vec2d(self.width / 2.0 * 0.8 - 4 * 2.0,
4 * 4.0), Vec2d(0, 1), float("inf")))
self.door_samples.append((Vec2d(self.width / 2.0 * 0.8 - 4 * 2.0,
4 * 4.0), Vec2d(0, -1), 5))
# polygon for left door
p6 = Vec2d(0, -line_width / 2.0)
p7 = Vec2d(0, line_width / 2.0)
p8 = Vec2d(self.width / 2.0 - line_width, line_width / 2.0)
p9 = Vec2d(self.width / 2.0 - line_width, -line_width / 2.0)
# ph1 = Vec2d(self.width/2.0*0.9, line_width/2.0)
# ph2 = Vec2d(self.width/2.0*0.9, line_width*4.0)
# ph3 = Vec2d(self.width/2.0*0.9-line_width*4.0, line_width*4.0)
# ph4 = Vec2d(self.width/2.0*0.9-line_width*4.0, line_width*3.0)
# ph5 = Vec2d(self.width/2.0*0.9-line_width*1.0, line_width*3.0)
# ph6 = Vec2d(self.width/2.0*0.9-line_width*1.0, line_width/2.0)
self.fp = [p9, p8, p7, p6]
mass = 100.0
moment = pymunk.moment_for_poly(mass, self.fp)
# left door
self.l_door_body = pymunk.Body(mass, moment)
self.l_door_body.position = t5
self.l_door_body.angle = self.rotation
l_door_shape = pymunk.Poly(self.l_door_body, self.fp, radius=1)
l_door_shape.group = 1
# handle
left_h1_shape = pymunk.Segment(
self.l_door_body, Vec2d(self.width / 2.0 * 0.8, line_width * 0.5),
Vec2d(self.width / 2.0 * 0.8, line_width * 4.0), line_width)
left_h1_shape.group = 1
left_h2_shape = pymunk.Segment(
self.l_door_body,
Vec2d(self.width / 2.0 * 0.8 - line_width * 4.0, line_width * 4.0),
Vec2d(self.width / 2.0 * 0.8, line_width * 4.0), line_width)
left_h2_shape.group = 1
space.add(self.l_door_body, l_door_shape, left_h1_shape, left_h2_shape)
j = pymunk.PivotJoint(self.l_door_body, space.static_body,
self.l_door_body.position)
s = pymunk.DampedRotarySpring(self.l_door_body, space.static_body, 0,
0, 90000)
space.add(j, s)
j2 = pymunk.RotaryLimitJoint(self.l_door_body, space.static_body,
-self.rotation - math.pi / 2.0,
-self.rotation)
space.add(j2)
def _setup_extra_bodies(self):
super()._setup_extra_bodies()
# Finger bodies/controls (annoying).
self.finger_bodies = []
self.finger_motors = []
self.finger_vertices = []
self.finger_inner_vertices = []
self.target_finger_angle = 0.0
for finger_side in [-1, 1]:
# Basic finger body.
finger_verts = make_finger_vertices(
upper_arm_len=self.finger_upper_length,
forearm_len=self.finger_lower_length,
thickness=self.finger_thickness,
side_sign=finger_side,
)
self.finger_vertices.append(finger_verts)
finger_inner_verts = make_finger_vertices(
upper_arm_len=self.finger_upper_length -
ROBOT_LINE_THICKNESS * 2,
forearm_len=self.finger_lower_length -
ROBOT_LINE_THICKNESS * 2,
thickness=self.finger_thickness - ROBOT_LINE_THICKNESS * 2,
side_sign=finger_side,
)
finger_inner_verts = [[(x, y + ROBOT_LINE_THICKNESS)
for x, y in box]
for box in finger_inner_verts]
self.finger_inner_vertices.append(finger_inner_verts)
# These are movement limits; they are useful below, but also
# necessary to make initial positioning work.
if finger_side < 0:
lower_rot_lim = -self.finger_rot_limit_inner
upper_rot_lim = self.finger_rot_limit_outer
if finger_side > 0:
lower_rot_lim = -self.finger_rot_limit_outer
upper_rot_lim = self.finger_rot_limit_inner
finger_mass = self.mass / 8
finger_inertia = pm.moment_for_poly(finger_mass,
sum(finger_verts, []))
finger_body = pm.Body(finger_mass, finger_inertia)
if finger_side < 0:
delta_finger_angle = upper_rot_lim
finger_body.angle = self.init_angle + delta_finger_angle
else:
delta_finger_angle = lower_rot_lim
finger_body.angle = self.init_angle + delta_finger_angle
# Position of finger relative to body.
finger_rel_pos = (
finger_side * self.radius * 0.45,
self.radius * 0.1,
)
finger_rel_pos_rot = gtools.rotate_vec(finger_rel_pos,
self.init_angle)
finger_body.position = gtools.add_vecs(self.body.position,
finger_rel_pos_rot)
self.add_to_space(finger_body)
self.finger_bodies.append(finger_body)
# Pivot joint to keep it in place (it will rotate around this
# point).
finger_piv = pm.PivotJoint(self.body, finger_body,
finger_body.position)
finger_piv.error_bias = 0.0
self.add_to_space(finger_piv)
# Rotary limit joint to stop it from getting too far out of line.
finger_limit = pm.RotaryLimitJoint(self.body, finger_body,
lower_rot_lim, upper_rot_lim)
finger_limit.error_bias = 0.0
self.add_to_space(finger_limit)
# Motor to move the fingers around (very limited in power so as not
# to conflict with rotary limit joint).
finger_motor = pm.SimpleMotor(self.body, finger_body, 0.0)
finger_motor.rate = 0.0
finger_motor.max_bias = 0.0
finger_motor.max_force = self.phys_vars.robot_finger_max_force
self.add_to_space(finger_motor)
self.finger_motors.append(finger_motor)
def fill_space(space, custom_color=(255, 255, 0, 255)):
captions = []
### Static
captions.append(((50, 680), "Static Shapes"))
#Static Segments
segments = [
pymunk.Segment(space.static_body, (10, 400), (10, 600), 0),
pymunk.Segment(space.static_body, (20, 400), (20, 600), 1),
pymunk.Segment(space.static_body, (30, 400), (30, 600), 3),
pymunk.Segment(space.static_body, (50, 400), (50, 600), 5)
]
space.add(segments)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (40, 630)
b.angle = 3.14 / 7
s = pymunk.Segment(b, (-30, 0), (30, 0), 2)
space.add(s)
# Static Circles
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 630)
s = pymunk.Circle(b, 10)
space.add(s)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 630)
s = pymunk.Circle(b, 10, (-30, 0))
space.add(s)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 560)
b.angle = 3.14 / 4
s = pymunk.Circle(b, 40)
space.add(s)
# Static Polys
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 460)
b.angle = 3.14 / 4
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)])
space.add(s)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 500)
t = pymunk.Transform(ty=-100)
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)], t, radius=3)
space.add(s)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (50, 430)
t = pymunk.Transform(ty=-100)
s = pymunk.Poly(b, [(0, -50), (50, 0), (30, 50), (-30, 50), (-50, 0)], t)
space.add(s)
### Kinematic
captions.append(((220, 680), "Kinematic Shapes"))
# Kinematic Segments
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
segments = [
pymunk.Segment(b, (180, 400), (180, 600), 0),
pymunk.Segment(b, (190, 400), (190, 600), 1),
pymunk.Segment(b, (200, 400), (200, 600), 3),
pymunk.Segment(b, (220, 400), (220, 600), 5)
]
space.add(segments)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (210, 630)
b.angle = 3.14 / 7
s = pymunk.Segment(b, (-30, 0), (30, 0), 2)
space.add(s)
# Kinematic Circles
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 630)
s = pymunk.Circle(b, 10)
space.add(s)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 630)
s = pymunk.Circle(b, 10, (-30, 0))
space.add(s)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 560)
b.angle = 3.14 / 4
s = pymunk.Circle(b, 40)
space.add(s)
# Kinematic Polys
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 460)
b.angle = 3.14 / 4
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)])
space.add(s)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 500)
t = pymunk.Transform(ty=-100)
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)], t, radius=3)
space.add(s)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (230, 430)
t = pymunk.Transform(ty=-100)
s = pymunk.Poly(b, [(0, -50), (50, 0), (30, 50), (-30, 50), (-50, 0)], t)
space.add(s)
### Dynamic
captions.append(((390, 680), "Dynamic Shapes"))
#Dynamic Segments
b = pymunk.Body(1, 1)
segments = [
pymunk.Segment(b, (350, 400), (350, 600), 0),
pymunk.Segment(b, (360, 400), (360, 600), 1),
pymunk.Segment(b, (370, 400), (370, 600), 3),
pymunk.Segment(b, (390, 400), (390, 600), 5),
]
space.add(segments)
b = pymunk.Body(1, 1)
b.position = (380, 630)
b.angle = 3.14 / 7
s = pymunk.Segment(b, (-30, 0), (30, 0), 2)
space.add(s)
# Dynamic Circles
b = pymunk.Body(1, 1)
b.position = (460, 630)
s = pymunk.Circle(b, 10)
space.add(s)
b = pymunk.Body(1, 1)
b.position = (460, 630)
s = pymunk.Circle(b, 10, (-30, 0))
space.add(s)
b = pymunk.Body(1, 1)
b.position = (460, 560)
b.angle = 3.14 / 4
s = pymunk.Circle(b, 40)
space.add(s)
# Dynamic Polys
b = pymunk.Body(1, 1)
b.position = (460, 460)
b.angle = 3.14 / 4
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)])
space.add(s)
b = pymunk.Body(1, 1)
b.position = (460, 500)
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)],
pymunk.Transform(ty=-100),
radius=3)
space.add(s)
b = pymunk.Body(1, 1)
b.position = (400, 430)
s = pymunk.Poly(b, [(0, -50), (50, 0), (30, 50), (-30, 50), (-50, 0)],
pymunk.Transform(ty=-100))
space.add(s)
###Constraints
# PinJoints
captions.append(((560, 660), "Pin Joints"))
a = pymunk.Body(1, 1)
a.position = (550, 600)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (650, 620)
sb = pymunk.Circle(b, 20)
j = pymunk.PinJoint(a, b)
space.add(sa, sb, a, b, j)
a = pymunk.Body(1, 1)
a.position = (550, 550)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (650, 570)
sb = pymunk.Circle(b, 20)
j = pymunk.PinJoint(a, b, anchor_a=(0, 20), anchor_b=(0, -20))
space.add(sa, sb, a, b, j)
# SlideJoints
captions.append(((560, 490), "Slide Joint"))
a = pymunk.Body(1, 1)
a.position = (550, 430)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (650, 450)
sb = pymunk.Circle(b, 20)
j = pymunk.SlideJoint(a,
b,
anchor_a=(0, 20),
anchor_b=(0, -20),
min=10,
max=30)
space.add(sa, sb, a, b, j)
# PivotJoints
captions.append(((560, 390), "Pivot Joint"))
a = pymunk.Body(1, 1)
a.position = (550, 330)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (650, 350)
sb = pymunk.Circle(b, 20)
j = pymunk.PivotJoint(a, b, (600, 320))
space.add(sa, sb, a, b, j)
# GrooveJoints
captions.append(((760, 660), "Groove Joint"))
a = pymunk.Body(1, 1)
a.position = (750, 600)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 620)
sb = pymunk.Circle(b, 20)
j = pymunk.GrooveJoint(a, b, (790, 610), (790, 620), (840, 620))
space.add(sa, sb, a, b, j)
# DampedSpring
captions.append(((760, 550), "Damped Spring"))
a = pymunk.Body(1, 1)
a.position = (750, 480)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 500)
sb = pymunk.Circle(b, 20)
j = pymunk.DampedSpring(a, b, (0, 0), (0, 10), 100, 1, 1)
space.add(sa, sb, a, b, j)
# DampedRotarySpring
captions.append(((740, 430), "Damped Rotary Spring"))
a = pymunk.Body(1, 1)
a.position = (750, 350)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 380)
sb = pymunk.Circle(b, 20)
j = pymunk.DampedRotarySpring(a, b, 10, 1, 1)
space.add(sa, sb, a, b, j)
# RotaryLimitJoint
captions.append(((740, 300), "Rotary Limit Joint"))
a = pymunk.Body(1, 1)
a.position = (750, 220)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 250)
sb = pymunk.Circle(b, 20)
j = pymunk.RotaryLimitJoint(a, b, 1, 2)
b.angle = 3
space.add(sa, sb, a, b, j)
# RatchetJoint
captions.append(((740, 170), "Ratchet Joint"))
a = pymunk.Body(1, 1)
a.position = (750, 100)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 120)
sb = pymunk.Circle(b, 20)
j = pymunk.RatchetJoint(a, b, 1, 0.1)
b.angle = 3
space.add(sa, sb, a, b, j)
# GearJoint and SimpleMotor omitted since they are similar to the already
# added joints
# TODO: more stuff here :)
### Other
# Objects in custom color
captions.append(((150, 150), "Custom Color (static & dynamic)"))
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (200, 200)
s = pymunk.Circle(b, 40)
s.color = custom_color
space.add(s)
b = pymunk.Body(1, 1)
b.position = (300, 200)
s = pymunk.Circle(b, 40)
s.color = custom_color
space.add(s)
# Collision
captions.append(((450, 150), "Collisions"))
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (470, 200)
s = pymunk.Circle(b, 40)
space.add(s)
b = pymunk.Body(1, 1)
b.position = (500, 250)
s = pymunk.Circle(b, 40)
space.add(s)
# Sleeping
captions.append(((50, 150), "Sleeping"))
b = pymunk.Body(1, 1)
b.position = (75, 200)
space.sleep_time_threshold = 0.01
s = pymunk.Circle(b, 40)
space.add(s, b)
b.sleep()
space.step(0.000001)
return captions
space.add(
pymunk.PivotJoint(b2, space.static_body, (150, 80) + Vec2d(box_offset)))
c = pymunk.DampedRotarySpring(b1, b2, 0, 3000, 60)
txts[box_offset] = inspect.getdoc(c)
space.add(c)
box_offset = 0, box_size
b1 = add_lever(space, (50, 100), box_offset)
b2 = add_lever(space, (150, 100), box_offset)
# Add some joints to hold the circles in place.
space.add(
pymunk.PivotJoint(b1, space.static_body, (50, 100) + Vec2d(box_offset)))
space.add(
pymunk.PivotJoint(b2, space.static_body, (150, 100) + Vec2d(box_offset)))
# Hold their rotation within 90 degrees of each other.
c = pymunk.RotaryLimitJoint(b1, b2, math.pi / 2, math.pi / 2)
txts[box_offset] = inspect.getdoc(c)
space.add(c)
box_offset = box_size, box_size
b1 = add_lever(space, (50, 100), box_offset)
b2 = add_lever(space, (150, 100), box_offset)
# Add some pin joints to hold the circles in place.
space.add(
pymunk.PivotJoint(b1, space.static_body, (50, 100) + Vec2d(box_offset)))
space.add(
pymunk.PivotJoint(b2, space.static_body, (150, 100) + Vec2d(box_offset)))
# Ratchet every 90 degrees
c = pymunk.RatchetJoint(b1, b2, 0, math.pi / 2)
txts[box_offset] = inspect.getdoc(c)
space.add(c)
def __init__(self, b, b2, min, max, collide=True):
joint = pymunk.RotaryLimitJoint(b, b2, min, max)
joint.collide_bodies = collide
space.add(joint)
def __init__(self, space: pymunk.Space, theta: int = 0):
'''
Generate swing and robot and add to space.
'''
self.space = space
# Define initial angle
angle = Angle(theta)
# Define bodies
self.top = pymunk.Body(50, 10000, pymunk.Body.STATIC)
self.bottom = pymunk.Body(50, 10000, pymunk.Body.STATIC)
self.rod1 = pymunk.Body(10, 10000)
# self.rod2 = pymunk.Body(5, 10000)
# self.rod3 = pymunk.Body(5, 10000)
self.seat = pymunk.Body(10, 10000)
self.torso = pymunk.Body(10, 10000)
self.legs = pymunk.Body(10, 10000)
self.head = pymunk.Body(5, 10000)
# Create shapes
self.top_shape = pymunk.Poly.create_box(self.top, size=(1200, 50))
self.bottom_shape = pymunk.Poly.create_box(self.bottom,
size=(1200, 50))
self.rod1_shape = pymunk.Segment(self.rod1, (0, 0),
angle.rod1,
radius=3)
# self.rod2_shape = pymunk.Segment(self.rod2, (0, 0), angle.rod2, radius=3)
# self.rod3_shape = pymunk.Segment(self.rod3, (0, 0), angle.rod3, radius=3)
self.seat_shape = pymunk.Segment(self.seat,
angle.seat[0],
angle.seat[1],
radius=5)
self.torso_shape = pymunk.Poly.create_box(self.torso,
size=(10, torso_length))
self.legs_shape = pymunk.Poly.create_box(self.legs,
size=(10, legs_length))
self.head_shape = pymunk.Circle(self.head, radius=20)
# Set layer of shapes (disables collisions between bodies)
self.rod1_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
# self.rod2_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
# self.rod3_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.seat_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.torso_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.legs_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.head_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
# Set positions of bodies
self.top.position = (600, 720)
self.bottom.position = (600, 0)
self.rod1.position = (600, 695)
# self.rod2.position = angle.point1
# self.rod3.position = angle.point2
self.seat.position = angle.point2
self.torso.position = (angle.point2[0] + angle.seat[1][0] +
(torso_length / 2) * math.sin(angle._theta),
angle.point2[1] + angle.seat[1][1] +
(torso_length / 2) * math.cos(angle._theta))
self.torso._set_angle(-angle._theta)
self.legs.position = (angle.point2[0] + angle.seat[0][0] -
(legs_length / 2) * math.sin(angle._theta),
angle.point2[1] + angle.seat[0][1] -
(legs_length / 2) * math.cos(angle._theta))
self.legs._set_angle(-angle._theta)
self.head.position = (angle.point2[0] + angle.seat[1][0] +
(torso_length - 10) * math.sin(angle._theta),
angle.point2[1] + angle.seat[1][1] +
(torso_length - 10) * math.cos(angle._theta))
# Create pivot joints for bodies
self.pivot1 = pymunk.PivotJoint(self.top, self.rod1, (600, 695))
self.pivot2 = pymunk.PivotJoint(self.rod1, self.seat, angle.point1)
# self.pivot3 = pymunk.PivotJoint(self.rod2, self.rod3, angle.point2)
# self.pivot4 = pymunk.PivotJoint(self.rod3, self.seat, angle.point4)
self.pivot5 = pymunk.PinJoint(self.rod1,
self.seat,
anchor_a=angle.rod1,
anchor_b=angle.seat[0])
self.pivot6 = pymunk.PinJoint(self.rod1,
self.seat,
anchor_a=angle.rod1,
anchor_b=angle.seat[1])
self.pivot7 = pymunk.PivotJoint(self.torso, self.seat,
(angle.point2[0] + angle.seat[1][0],
angle.point2[1] + angle.seat[1][1]))
self.pivot8 = pymunk.PivotJoint(self.seat, self.legs,
(angle.point2[0] + angle.seat[0][0],
angle.point2[1] + angle.seat[0][1]))
self.pivot9 = pymunk.PivotJoint(self.head, self.torso,
self.head.position)
# self.arms = pymunk.DampedSpring(self.torso, self.rod3, anchor_a=(0,0), anchor_b=(0,0), rest_length=25, stiffness=100, damping=0.5)
self.gear1 = pymunk.RotaryLimitJoint(self.torso, self.seat, -0.1, 0.5)
self.gear2 = pymunk.RotaryLimitJoint(self.seat, self.legs, -1.2, 0.9)
# Disable collisions between rods
self.pivot1.collide_bodies = False
self.pivot2.collide_bodies = False
# self.pivot3.collide_bodies = False
# self.pivot4.collide_bodies = False
self.pivot7.collide_bodies = False
self.pivot8.collide_bodies = False
self.pivot9.collide_bodies = False
# Add bodies and pivots to space
self.space.add(self.top, self.top_shape, self.bottom,
self.bottom_shape, self.rod1, self.rod1_shape,
self.seat, self.seat_shape, self.torso,
self.torso_shape, self.legs, self.legs_shape, self.head,
self.head_shape, self.pivot1, self.pivot2, self.pivot5,
self.pivot6, self.pivot7, self.pivot8, self.pivot9,
self.gear1, self.gear2)
