Mercurial > katka

changeset 1:568e7573fa8c

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Basic inverse kinematics
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Thu, 11 Sep 2014 21:41:08 +0200
parents 327a77c0cd71
children 10ddd812378b
files home.py katka.py leg.py robot.py servo.py swing.py
diffstat 6 files changed, 252 insertions(+), 0 deletions(-) [+]
line wrap: on
line diff
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/home.py	Thu Sep 11 21:41:08 2014 +0200
@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import katka
+
+
+robot = katka.Robot()
+robot.update()
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/katka.py	Thu Sep 11 21:41:08 2014 +0200
@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import serial
+
+import robot
+import servo
+import leg
+
+
+class Leg(leg.Leg2D):
+    # Lengths of leg segments, in mm
+    femur = 38.0  # between the knee and the ankle
+    tibia = 35.0  # between the ankle and the foot
+    base = 22.0, 45.0  # from the center of body
+
+
+class Servo(servo.Servo):
+    span = 180
+    center = 90
+
+
+class Robot(robot.Robot):
+    def __init__(self):
+        s = serial.Serial('/dev/ttyUSB0', 115200)
+        self.legs = (
+            # front left
+            Leg(Servo(s, 1, reverse=False, trim=5),
+                Servo(s, 0, reverse=False, trim=5), -1),
+            # front right
+            Leg(Servo(s, 6, reverse=False),
+                Servo(s, 7, reverse=False, trim=-5), -1),
+            # hind left
+            Leg(Servo(s, 3, reverse=True),
+                Servo(s, 2, reverse=True, trim=5), 1),
+            # hind right
+            Leg(Servo(s, 4, reverse=True),
+                Servo(s, 5, reverse=True, trim=-5), 1),
+        )
+        super(Robot, self).__init__()
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/leg.py	Thu Sep 11 21:41:08 2014 +0200
@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import math
+
+
+SQRT2 = math.sqrt(2)
+
+
+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)
+
+
+class Leg2D(object):
+    """A signle leg, containing 2 servos: knee and ankle, and two
+       connections between them: femur and tibia.
+    """
+
+    # Lengths of leg segments, in mm
+    femur = 38.0  # between the knee and the ankle
+    tibia = 30.0  # between the ankle and the foot
+    base = 15.0, 15.0  # from the center of body
+
+    def __init__(self, knee, ankle, x_dir):
+        self.knee = knee
+        self.ankle = ankle
+        self.x_dir = x_dir
+        self.home = [
+            self.tibia / SQRT2,
+            self.femur + self.tibia / SQRT2,
+        ]
+        self.position = self.home
+
+    def __iter__(self):
+        """Iterate over all joints."""
+        return iter([self.knee, self.ankle])
+
+    def _inverse_kinematics(self, (x, y)):
+        """Calculate the 3 angles of the leg, in degrees."""
+        d = _length(x, y)
+        knee = (math.atan2(y, x) +
+                _solve_triangle(self.femur, d, self.tibia))
+        ankle = _solve_triangle(self.femur, self.tibia, d)
+        knee = 90 - math.degrees(knee)
+        ankle = math.degrees(ankle) - 180 + 45
+        return 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)):
+        """Move the foot by the specified amount."""
+        x, y = self.position
+        self.move_to((x + dx, y + dy))
+
+    def update(self, shift=(0, 0), send=True):
+        """Send the positions to the servos."""
+        dx, dy = shift
+        x, y = self.position
+        position = x + dx * self.x_dir, y + dy
+        angles = self._inverse_kinematics(position)
+        for joint, angle in zip(self, angles):
+            joint.move_degrees(angle)
+            if send:
+                joint.update()
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.py	Thu Sep 11 21:41:08 2014 +0200
@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import pygame
+
+
+class Robot(object):
+    legs = ()
+
+    def __init__(self):
+        self.shift = [0.0, 0.0]
+        self.clock = pygame.time.Clock()
+
+    def update(self, fps=12, at_once=True):
+        servos = []
+        for leg in self.legs:
+            leg.update(self.shift, send=not at_once)
+            servos.extend(leg)
+        if at_once:
+            self.legs[0].ankle.update_all(servos)
+        self.clock.tick(fps)
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo.py	Thu Sep 11 21:41:08 2014 +0200
@@ -0,0 +1,60 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+
+def _normalize_degrees(angle):
+    """Convert an angle to range +-180 degree range."""
+    while angle > 180:
+        angle -= 360
+    while angle < -180:
+        angle += 360
+    return angle
+
+def _word(value):
+    value = int(value)
+    return chr(value & 0xff) + (chr((value & 0xff00) >> 8))
+
+
+class Servo(object):
+    span = 180
+    center = 90
+    min_freq = 600
+    max_freq = 2400
+
+    def __init__(self, serial, index, reverse=False, trim=0):
+        self.index = index
+        self.position = self.center
+        self.reverse = reverse
+        self.trim = trim
+        self.serial = serial
+
+    def update(self):
+        freq = self.calculate_freq()
+        command = '\xfd%c%s' % (self.index, _word(freq))
+        self.serial.write(command)
+
+    @classmethod
+    def update_all(cls, servos):
+        serial = servos[0].serial
+        servos = sorted(servos, key=lambda servo: servo.index)
+        command = '\xfc%s' % ''.join(_word(servo.calculate_freq())
+                                     for servo in servos)
+        serial.write(command)
+
+    def calculate_angle(self):
+        position = self.position
+        if self.reverse:
+            position = -position
+        angle = min(max(0, self.center + position + self.trim), self.span)
+        return angle
+
+    def calculate_freq(self):
+        angle = self.calculate_angle()
+        return int(self.min_freq +
+                   angle * (self.max_freq - self.min_freq) / self.span)
+
+    def move_degrees(self, degrees):
+        degrees = _normalize_degrees(degrees)
+        if not self.center - self.span <= degrees <= self.center + self.span:
+            raise ValueError("Servo angle out of range.")
+        self.position = degrees
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/swing.py	Thu Sep 11 21:41:08 2014 +0200
@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import katka
+
+
+FPS = 24
+
+
+def swing(robot):
+    for leg in robot.legs:
+        leg.move_to((None, 53))
+    yield
+    dx, dy = 10, 10
+    while True:
+        for x in range(0, dx, 1):
+            robot.shift[0] = x
+            yield
+        for y in range(0, dy, 1):
+            robot.shift[1] = y
+            yield
+        for x in range(dx, -dx, -1):
+            robot.shift[0] = x
+            yield
+        for y in range(dy, -dy, -1):
+            robot.shift[1] = y
+            yield
+        for x in range(-dx, 0, 1):
+            robot.shift[0] = x
+            yield
+        for y in range(-dy, 0, 1):
+            robot.shift[1] = y
+            yield
+
+robot = katka.Robot()
+for frame in swing(robot):
+    robot.update(FPS)
+