--- a/kubik.py	Tue Jul 15 00:51:47 2014 +0200
+++ b/kubik.py	Tue Jul 15 22:56:33 2014 +0200
@@ -57,9 +57,9 @@
     """
 
     # Lengths of leg segments, in mm
-    coxa = 36.0  # between the hip and the knee
+    coxa = 40.0  # between the hip and the knee
     femur = 30.0  # between the knee and the ankle
-    tibia = 36.0  # between the ankle and the foot
+    tibia = 36.5  # between the ankle and the foot
     base = 45.0, 45.0  # from the center of body
     home = 45.0, 45.0, -35.0
 
@@ -73,8 +73,16 @@
         else:
             self.y_direction = -1
         self.hip_offset = -45 * self.y_direction
-        self.position = (
+        self.home = (
             self.home[0], self.home[1] * self.y_direction, self.home[2])
+        self.position = list(self.home)
+
+    def __repr__(self):
+        return "<Leg (%r, %r, %r)>" % (
+            self.position[0],
+            self.position[1],
+            self.position[2],
+        )
 
     def __iter__(self):
         """Iterate over all joints."""
@@ -111,7 +119,7 @@
 
     def rotate(self, degrees):
         """Rotate the leg around the center of the body horizontally."""
-        radians = math.radians(degrees)
+        radians = math.radians(degrees * self.y_direction)
         x, y, z = self.position
         bx = self.base[0] + x
         by = self.base[1] + y * self.y_direction
@@ -173,18 +181,18 @@
         ]
         for leg, shift in zip(self.legs, shifts):
             x, y, z = leg.position
-            if (x, y) != (leg.home[0], leg.home[1] * leg.y_direction):
+            if (x, y) != (leg.home[0], leg.home[1]):
                 self.shift = shift
                 leg.move_to((None, None, 5))
                 for i in range(2):
                     yield
-                leg.move_to((leg.home[0], leg.home[1] * leg.y_direction, None))
+                leg.move_to((leg.home[0], leg.home[1], None))
                 for i in range(2):
                     yield
                 leg.move_to((None, None, leg.home[2] + 1))
                 for i in range(4):
                     yield
-            leg.move_to((None, None, leg.home[2]))
+            leg.move_to(leg.home)
         self.shift = [0, 0, 0]
 
     def home(self):

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/walk.py	Tue Jul 15 22:56:33 2014 +0200
@@ -0,0 +1,104 @@
+import pygame
+
+import kubik
+
+
+FPS = 12
+
+
+class Walk(object):
+    def __init__(self, robot):
+        self.robot = robot
+        self.speed = [0, 0]
+        self.rotation = 1.0
+        self.stride = 35
+        self.shift_speed = 3
+        self.shift_steps = 4
+        self.down = -40
+        self.up = -20
+        self.legs = [
+            (self.robot.legs[0], (1, -1), 1, (35, -35)),
+            (self.robot.legs[1], (1, 1), 1, (35, 35)),
+            (self.robot.legs[3], (-1, -1), -1, (35, -35)),
+            (self.robot.legs[2], (-1, 1), -1, (35, 35)),
+        ]
+        self.on_floor = set((leg, side)
+                            for leg, shift, side, home in self.legs)
+
+    def _retract_leg(self, step):
+        dx, dy = self.speed
+        leg, side = step
+        leg.move_by((-dx * side, -dy, 0))
+        leg.rotate(self.rotation * side / 3)
+
+    def _retract(self):
+        while True:
+            for step in self.on_floor:
+                self._retract_leg(step)
+            yield
+
+    def _raise_leg(self, leg, side):
+        leg.move_to((None, None, self.up))
+        self.on_floor.remove((leg, side))
+
+    def _advance_leg(self, leg, side, home):
+        dx, dy = self.speed
+        leg.move_to((
+            home[0] + dx * self.stride * side - (abs(dx) + abs(dy)) * 15,
+            home[1] + dy * self.stride - (
+                abs(dy) + abs(dx)) * 15 * leg.y_direction,
+            None,
+        ))
+        leg.rotate(-self.rotation * self.stride * side / 3)
+
+    def _lower_leg(self, leg):
+        leg.move_to((None, None, self.down + 1))
+
+    def _ground_leg(self, leg, side):
+        leg.move_to((None, None, self.down))
+        self.on_floor.add((leg, side))
+
+    def _stomp(self):
+        shifted = 0, 0
+        while True:
+            for leg, shift, side, home in self.legs:
+                for frame in range(self.shift_steps):
+                    if shift[0] != shifted[0]:
+                        self.robot.shift[0] += shift[0] * self.shift_speed
+                    if shift[1] != shifted[1]:
+                        self.robot.shift[1] += shift[1] * self.shift_speed
+                    yield
+                self._raise_leg(leg, side)
+                for frame in range(2):
+                    yield
+                self._advance_leg(leg, side, home)
+                self._lower_leg(leg)
+                for frame in range(4):
+                    yield
+                self._ground_leg(leg, side)
+                for frame in range(self.shift_steps):
+                    if shift[0] != shifted[0]:
+                        self.robot.shift[0] -= shift[0] * self.shift_speed
+                    if shift[1] != shifted[1]:
+                        self.robot.shift[1] -= shift[1] * self.shift_speed
+                    yield
+                shifted = shift
+
+    def run(self):
+        clock = pygame.time.Clock()
+        stomp = self._stomp()
+        retract = self._retract()
+        try:
+            while True:
+                stomp.next()
+                retract.next()
+                robot.update()
+                clock.tick(FPS)
+        finally:
+            self.robot.home()
+
+
+if __name__ == '__main__':
+    robot = kubik.Robot()
+    walk = Walk(robot)
+    walk.run()