Inverse Kinematics for Inverted Cantilever Delta printer

ik.py

#!/usr/bin/env python

from __future__ import print_function

import math

Motor= ["X", "Y", "Z"]  # Motor axes


class Vector:
    def __init__(self, x=0.0, y=0.0, z=0.0):
        self._x = x
        self._y = y
        self._z = z

    def __repr__(self):
        return "X{}, Y{}, Z{}".format(self._x, self._y, self._z)

    def __add__(self, other):
        return Vector(self._x + other.x, self._y + other.y, self._z + other.z)

    def __sub__(self, other):
        return Vector(self._x - other.x, self._y - other.y, self._z - other.z)

    def translateXY(self, offset):
        return Vector(self.x + offset.x, self.y + offset.y, self.z)

    def translateXYZ(self, offset):
        return Vector(self.x + offset.x, self.y + offset.y, self.z + offset.z)

    def rotateXY(self, angle, origin=None):
        if origin is None:
            origin = Vector()

        # Translate XY
        x = self.x - origin.x
        y = self.y - origin.y

        # Rotate
        xr = (x * math.cos(angle)) - (y * math.sin(angle))
        yr = (y * math.cos(angle)) + (x * math.sin(angle))

        # Translate back and return
        return Vector(xr + origin.x, yr + origin.y, self.z)

    @property
    def length3D(self):
        return math.sqrt(self.x2 + self.y2 + self.z2)

    @property
    def length2D(self):
        return math.sqrt(self.x2 + self.y2)

    @property
    def x(self):
        return self._x

    @x.setter
    def x(self, x):
        self._x = x

    @property
    def x2(self):
        return math.pow(self._x, 2)

    @property
    def y(self):
        return self._y

    @y.setter
    def y(self, y):
        self._y = y

    @property
    def y2(self):
        return math.pow(self._y, 2)

    @property
    def z(self):
        return self._z

    @z.setter
    def z(self, z):
        self._z = z

    @property
    def z2(self):
        return math.pow(self._z, 2)


class Arm:
    def __init__(self, factor, index, position):
        self.factor = factor  # 2*length^2 constant
        self.position = position  # Cartesian position
        self.theta = self._solve(position)  # Starting angle
        self.index = index  # Alpha/Beta/Gamma arm

    def _solve(self, dest):
        # Reference kinematics
        # 2 * length^2 * (1 - cos(theta)) = x^2 + y^2 + z^2
        # theta = arccos(1 - (x^2 + y^2 + z^2) / (2*length^2))

        # Add vector component squares
        v2 = dest.x2 + dest.y2 + dest.z2

        # Take arc cosine and return theta
        return math.acos(1.0 - (v2 / self.factor))

    def move(self, dest):
        # Translate destination to arm position
        dest = dest.translateXYZ(self.position)

        # Update position
        self.position = dest

        # Solve IK
        theta = self._solve(dest)

        # Get difference with previous theta
        diff = self.theta - theta

        # Update to new angle
        self.theta = theta

        # Output gcode for move
        return "{}{}".format(Motor[self.index], math.degrees(diff * Scale))


class Printer:
    def __init__(self, length):
        self.length = length  # arm length in mm
        self.factor = 2.0 * math.pow(self.length, 2)  # 2*l^2
        self._home = Vector(0, 0, 0) # home position

        # Setup arms, translating by length/height and rotating delta arms
        self.alpha = Arm(self.factor, 0, self._home.translateXYZ(Vector(length, 0, length)))
        self.beta = Arm(self.factor, 1, self.alpha.position.rotateXY(math.radians(120)))
        self.gamma = Arm(self.factor, 2, self.alpha.position.rotateXY(math.radians(240)))

    def home(self):
        self.move(self._home)

    def move(self, v):
        #print("Offset: {}".format(v - self._home))
        #print("Rotate: {} {} {}".format(self.alpha.move(v), self.beta.move(v), self.gamma.move(v)))
        print("G0 {} {} {} F{}".format(self.alpha.move(v), self.beta.move(v), self.gamma.move(v), Rate))


Rate = 500  # Feed rate
Scale = 1  # deg/step / gear ratio

# Gcode prologue
print("G91")  # Set relative positioning
print("G92 X0 Y0 Z0")  # Reset positions
print("M92 X100 Y100 Z100") # Set steps/mm scaling

# Create printer with 150mm arms
p = Printer(150)

# Do moves
p.home()  # Go home
p.move(Vector(-10, 0, 0))  # -x
p.move(Vector(20, 0, 0))  # +x
p.move(Vector(-10, 0, 0))  # -x
p.move(Vector(0, -10, 0))  # -y
p.move(Vector(0, 20, 0))  # +y
p.move(Vector(0, -10, 0))  # -y
p.move(Vector(0, 0, -10))  # -z
p.move(Vector(0, 0, 20))  # +z
p.move(Vector(0, 0, -10))  # -z

output.txt

Offset: X0, Y0, Z0
Rotate: A0.0 B0.0 G0.0
Offset: X-10, Y0, Z0
Rotate: A3.69495527379 B-2.03761274962 G-2.03761274962
Offset: X10, Y0, Z0
Rotate: A-3.95017117409 B1.78282304098 G1.78282304098
Offset: X0, Y-10, Z0
Rotate: A-0.127324059267 B3.18228530708 G-3.43735890595
Offset: X0, Y10, Z0
Rotate: A-0.127324059267 B-3.43735890595 G3.18228530708
Offset: X0, Y-10, Z0
Rotate: A-0.127324059267 B3.18228530708 G-3.43735890595
Offset: X0, Y10, Z0
Rotate: A-0.127324059267 B-3.43735890595 G3.18228530708
Offset: X0, Y0, Z-10
Rotate: A3.69495527379 B3.69495527379 G3.69495527379
Offset: X0, Y0, Z10
Rotate: A-3.95017117409 B-3.95017117409 G-3.95017117409