--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/leg.py	Wed Aug 06 18:11:43 2014 +0200
@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import math
+
+def _solve_triangle(a, b, c):
+    """Calculate an angle of a triangle using the law of cosines."""
+    a, b, c = abs(a), abs(b), abs(c)
+    if a + b < c or a + c < b or b + c < a:
+        raise ValueError("Impossible triangle: %r, %r, %r" % (a, b, c))
+    cos = float(a ** 2 + b ** 2 - c ** 2) / (2 * a * b)
+    return math.acos(cos)
+
+
+def _length(a, b):
+    """Calculate the length of a vector."""
+    return math.sqrt(a ** 2 + b ** 2)
+
+
+def _normalize_degrees(angle):
+    """Convert an angle to range +-180 degree range."""
+    while angle > 180:
+        angle -= 360
+    while angle < -180:
+        angle += 360
+    return angle
+
+
+class Leg(object):
+    """A signle leg, containing 3 servos: hip, knee and ankle, and three
+       connections between them: coxa, femur and tibia.
+    """
+
+    # Lengths of leg segments, in mm
+    coxa = 16.0  # between the hip and the knee
+    femur = 50.0  # between the knee and the ankle
+    tibia = 45.0  # between the ankle and the foot
+    base = 45.0, 45.0  # from the center of body
+    home = 45.0, 45.0, -35.0
+
+    def __init__(self, hip, knee, ankle, front=True):
+        self.hip = hip
+        self.knee = knee
+        self.ankle = ankle
+        self.front = front
+        if self.front:
+            self.y_direction = 1
+        else:
+            self.y_direction = -1
+        self.hip_offset = -45 * self.y_direction
+        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."""
+        return iter([self.hip, self.knee, self.ankle])
+
+    def _inverse_kinematics(self, (x, y, z)):
+        """Calculate the 3 angles of the leg, in radians."""
+        f = _length(x, y) - self.coxa
+        d = min(self.femur + self.tibia, _length(f, z))
+        hip = math.atan2(y, x)
+        knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f)
+        ankle = _solve_triangle(self.femur, self.tibia, d)
+        knee = _normalize_degrees(-math.degrees(knee))
+        ankle = _normalize_degrees(math.degrees(ankle) - 90)
+        hip = _normalize_degrees(math.degrees(hip) + self.hip_offset)
+        return hip, knee, ankle
+
+    def move_to(self, position):
+        """Move the foot to the specified point.
+
+        If any of the coordinates is None, no movement will be done in that
+        axis.
+        """
+        position = list(position)
+        for i, coord in enumerate(position):
+            if coord is None:
+                position[i] = self.position[i]
+        self.position = position
+
+    def move_by(self, (dx, dy, dz)):
+        """Move the foot by the specified amount."""
+        x, y, z = self.position
+        self.move_to((x + dx, y + dy, z + dz))
+
+    def rotate(self, degrees):
+        """Rotate the leg around the center of the body horizontally."""
+        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
+        r = _length(bx, by)
+        a = math.atan2(bx, by)
+        rx = r * math.sin(a + radians) - self.base[0]
+        ry = r * math.cos(a + radians) - self.base[1]
+        self.move_to((rx, ry * self.y_direction, None))
+
+    def update(self, shift):
+        dx, dy, dz = shift
+        x, y, z = self.position
+        hip, knee, ankle = self._inverse_kinematics((x + dx, y + dy, z + dz))
+        self.hip.move_degrees(hip)
+        self.knee.move_degrees(knee)
+        self.ankle.move_degrees(ankle)