Why is the physics wrong in the following Pymunk example?
from __future__ import print_function
import sys
from math import pi
import pygame
from pygame.locals import USEREVENT, QUIT, KEYDOWN, KEYUP, K_s, K_r, K_q, K_ESCAPE, K_UP, K_DOWN, K_LEFT, K_RIGHT
from pygame.color import THECOLORS
import pymunk
from pymunk import Vec2d
import pymunk.pygame_util
LEG_GROUP = 1
class Simulator(object):
def __init__(self):
self.display_flags = 0
self.display_size = (600, 600)
self.space = pymunk.Space()
self.space.gravity = (0.0, -1900.0)
self.space.damping = 0.999 # to prevent it from blowing up.
# Pymunk physics coordinates start from the lower right-hand corner of the screen.
self.ground_y = 100
ground = pymunk.Segment(self.space.static_body, (5, self.ground_y), (595, self.ground_y), 1.0)
ground.friction = 1.0
self.space.add(ground)
self.screen = None
self.draw_options = None
def reset_bodies(self):
for body in self.space.bodies:
if not hasattr(body, 'start_position'):
continue
body.position = Vec2d(body.start_position)
body.force = 0, 0
body.torque = 0
body.velocity = 0, 0
body.angular_velocity = 0
body.angle = body.start_angle
def draw(self):
### Clear the screen
self.screen.fill(THECOLORS["white"])
### Draw space
self.space.debug_draw(self.draw_options)
### All done, lets flip the display
pygame.display.flip()
def main(self):
pygame.init()
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
# leg_length = 100
leg_length = 125
leg_thickness = 2
leg_shape_filter = pymunk.ShapeFilter(group=LEG_GROUP)
# Create torso.
mass = 200
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(mass, points)
body1 = pymunk.Body(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
shape1 = pymunk.Poly(body1, 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.
mass = 10
points = [
(leg_thickness/2, -leg_length/2),
(-leg_thickness/2, -leg_length/2),
(-leg_thickness/2, leg_length/2),
(leg_thickness/2, leg_length/2)
]
moment = pymunk.moment_for_poly(mass, points)
body2 = pymunk.Body(mass, moment)
body2.position = (self.display_size[0]/2-box_width/2+leg_thickness/2, self.ground_y+leg_length/2)
body2.start_position = Vec2d(body2.position)
body2.start_angle = body2.angle
shape2 = pymunk.Poly(body2, 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))
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.
pj = pymunk.PivotJoint(self.space.static_body, body2, (self.display_size[0]/2-box_width/2, self.ground_y+leg_thickness))
self.space.add(pj)
# Actuate the bars via a motor.
motor_joint = pymunk.SimpleMotor(body1, body2, 0)
motor_joint.max_force = 1e10 # mimicks default infinity
# motor_joint.max_force = 1e9
# motor_joint.max_force = 1e7 # too weak, almost no movement
self.space.add(motor_joint)
# Add hard stops to leg pivot so the leg can't rotate through the torso.
hip_limit_joint = pymunk.RotaryLimitJoint(body1, body2, -pi/4., pi/4.) # -45deg:+45deg
self.space.add(hip_limit_joint)
last_body1_pos = None
last_body1_vel = None
simulate = False
while running:
# 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))
servo_angle = (body1.angle - body2.angle) * 180/pi # 0 degrees means leg is angled straight down
servo_cw_enabled = servo_angle > -45
servo_ccw_enabled = servo_angle < 45
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYDOWN and event.key in (K_q, K_ESCAPE)):
sys.exit(0)
elif event.type == KEYDOWN and event.key == K_s:
# Start/stop simulation.
simulate = not simulate
last_body1_pos = Vec2d(body1.position)
if current_body1_vel:
last_body1_vel = Vec2d(current_body1_vel)
self.draw()
### 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)
pygame.display.flip()
clock.tick(fps)
if __name__ == '__main__':
sim = Simulator()
sim.main()
This renders a box placed on top of a thin leg. The leg is connected to the box by a pivot joint, and to the ground via another pivot joint. However, the leg is attached to the box off-center to the left, so the center-of-gravity is unbalanced. In the real world, this setup would cause the box to toppled to the right. However, when you run this code (and press "s" to start), it shows the box toppling to the left. Why is this?
I've tried adjusting the mass (high mass for the box, low mass for the leg), the center of gravity for the box, and tweaking the attachment points for the joints, but nothing seems to change the outcome. What am I doing wrong?
I want to use this for simulating a real-world phenomena, but until I can get it to reproduce the real-world phenomena, I'm stuck.
It seems to be because the leg shape collides with the bottom ground shape.
The easiest way to make them not collide is to move them apart a little bit. For example make the leg a little shorter so that it doesnt touch the ground.
Another solution is to do as you did in your other question, ignore collisions between the leg and the ground. To do that you can setup a shape filter, but since you probably want to keep the box from colliding with the leg, and at the same time count collisions between the box and ground I think you need to use the categories/masks of the shape filter as documented here: http://www.pymunk.org/en/latest/pymunk.html#pymunk.ShapeFilter