title: How to fix unbalanced falling kinematics in Pymunk
body: <p>Why is the physics wrong in the following Pymunk example?</p>
  
  <pre><code>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 &gt; -45
              servo_ccw_enabled = servo_angle &lt; 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()
  </code></pre>
  
  <p>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?</p>
  
  <p>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?</p>
  
  <p>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.</p>