""" matrix.py: Matrix class for matrix operations.

"""
# Built for STEWART FORCE>..

from math import *

import random
import operator
import sys
import unittest
import numpy as np

__version__ = "0.47"
print('Stewart Platform API v0.47\n');

def rotation_z(angleZ):
sinZ = sin(angleZ)
cosZ = cos(angleZ)

rot = np.array([[cosZ, -sinZ, 0], [sinZ, cosZ, 0], [0, 0, 1]])

return rot

def rotation(angleX, angleY, angleZ):

cosX = cos(angleX);
sinX = sin(angleX);
cosY = cos(angleY);
sinY = sin(angleY);
cosZ = cos(angleZ);
sinZ = sin(angleZ);
m_sinX = -sinX;
m_sinY = -sinY;
m_sinZ = -sinZ;

rot = np.array([[cosY * cosZ, m_sinX * m_sinY * cosZ + cosX * sinZ, cosX *

m_sinY * cosZ + sinX * sinZ, 0],
[cosY * m_sinZ, m_sinX * m_sinY * m_sinZ + cosX * cosZ, cosX *
m_sinY * m_sinZ + sinX * cosZ, 0],
[sinY, m_sinX * cosY, cosX * cosY, 0],
[0, 0, 0, 1]])
return rot

def translation(x, y, z):

trans = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [x, y, z, 1]])
return trans

class MatrixError(Exception):
""" An exception class for Matrix """
pass

class Matrix(object):
""" A simple Python matrix class with
basic operations and operator overloading """

def __init__(self, m, n, init=True):

if init:
 self.rows = [[0] * n for x in range(m)]
else:
self.rows = []
self.m = m
self.n = n

def __getitem__(self, idx):

return self.rows[idx]

def __setitem__(self, idx, item):

self.rows[idx] = item

def __str__(self):
s = '\n'.join([' '.join([str(item) for item in row]) for row in self.rows])
return s + '\n'

def __repr__(self):
s = str(self.rows)
rank = str(self.getRank())
rep = "Matrix: \"%s\", rank: \"%s\"" % (s, rank)
return rep

def reset(self):
""" Reset the matrix data """
self.rows = [[] for x in range(self.m)]
return self;

def transpose(self):
""" Transpose the matrix. Changes the current matrix """

self.m, self.n = self.n, self.m

self.rows = [list(item) for item in zip(*self.rows)]
return self;

def getTranspose(self):
""" Return a transpose of the matrix without
modifying the matrix itself """

m, n = self.n, self.m
mat = Matrix(m, n)
mat.rows = [list(item) for item in zip(*self.rows)]

return mat

def getRank(self):
return (self.m, self.n)

def __eq__(self, mat):

""" Test equality """

return (mat.rows == self.rows)

def __add__(self, mat):

""" Add a matrix to this matrix and
return the new matrix. Doesn't modify
the current matrix """

if self.getRank() != mat.getRank():
raise MatrixError("Trying to add matrixes of varying rank!")
 ret = Matrix(self.m, self.n)

for x in range(self.m):
row = [sum(item) for item in zip(self.rows[x], mat[x])]
ret[x] = row

return ret

def __sub__(self, mat):

""" Subtract a matrix from this matrix and
return the new matrix. Doesn't modify
the current matrix """

if self.getRank() != mat.getRank():
raise MatrixError("Trying to add matrixes of varying rank!")

ret = Matrix(self.m, self.n)

for x in range(self.m):
row = [item[0] - item[1] for item in zip(self.rows[x], mat[x])]
ret[x] = row

return ret

def __mul__(self, mat):

""" Multiple a matrix with this matrix and
return the new matrix. Doesn't modify
the current matrix """

matm, matn = mat.getRank()

if (self.n != mat.m):
raise MatrixError("Matrices cannot be multipled!")

mat_t = mat.getTranspose()
mulmat = Matrix(self.m, matn)

for x in range(self.m):
for y in range(mat_t.m):
mulmat[x][y] = sum([item[0] * item[1] for item in zip(self.rows[x],
mat_t[y])])

return mulmat

def __iadd__(self, mat):

""" Add a matrix to this matrix.
This modifies the current matrix """

# Calls __add__
tempmat = self + mat
self.rows = tempmat.rows[:]
return self

def __isub__(self, mat):

""" Add a matrix to this matrix.
This modifies the current matrix """

# Calls __sub__
 tempmat = self - mat
self.rows = tempmat.rows[:]
return self

def __imul__(self, mat):

""" Add a matrix to this matrix.
This modifies the current matrix """

# Possibly not a proper operation

# since this changes the current matrix
# rank as well...

# Calls __mul__
tempmat = self * mat
self.rows = tempmat.rows[:]
self.m, self.n = tempmat.getRank()
return self

def save(self, filename):

open(filename, 'w').write(str(self))

@classmethod
def _makeMatrix(cls, rows):

m = len(rows)
n = len(rows[0])
# Validity check
if any([len(row) != n for row in rows[1:]]):
raise MatrixError("inconsistent row length")
mat = Matrix(m, n, init=False)
mat.rows = rows

return mat

@classmethod
def makeRandom(cls, m, n, low=0, high=10):
""" Make a random matrix with elements in range (low-high) """

obj = Matrix(m, n, init=False)

for x in range(m):
obj.rows.append([random.randrange(low, high) for i in range(obj.n)])

return obj

@classmethod
def makeZero(cls, m, n):
""" Make a zero-matrix of rank (mxn) """

rows = [[0] * n for x in range(m)]

return cls.fromList(rows)

@classmethod
def makeDiagonal(cls, m, n):
""" Make a diagonal matrix (mxn) """
if m != n:
raise MatrixError("Only sqaure matrices can be made diagonal")

rows = cls.makeZero(m, n)
for i in range(m): rows[i][i] = 1
 return cls.fromList(rows)

@classmethod
def makeId(cls, m):
""" Make identity matrix of rank (mxm) """

rows = [[0] * m for x in range(m)]

idx = 0

for row in rows:

row[idx] = 1
idx += 1

return cls.fromList(rows)

@classmethod
def readStdin(cls):
""" Read a matrix from standard input """

print('Enter matrix row by row. Type "q" to quit')

rows = []
while True:
line = sys.stdin.readline().strip()
if line == 'q': break

row = [int(x) for x in line.split()]

rows.append(row)

return cls._makeMatrix(rows)

@classmethod
def readGrid(cls, fname):
""" Read a matrix from a file """

rows = []
for line in open(fname).readlines():
row = [int(x) for x in line.split()]
rows.append(row)

return cls._makeMatrix(rows)

@classmethod
def fromList(cls, listoflists):
""" Create a matrix by directly passing a list
of lists """

# E.g: Matrix.fromList([[1 2 3], [4,5,6], [7,8,9]])

rows = listoflists[:]
return cls._makeMatrix(rows)

from dynamixel_sdk import *

def wait(s):
sleep(s)
def map(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

class Actuators:
def __init__(self, port='COM14', baudrate=1000000):
self.portHandler = PortHandler(port)
self.packetHandler = PacketHandler(1.0)
self.groupSyncWrite = GroupSyncWrite(self.portHandler, self.packetHandler,
30, 4)
self.portHandler.openPort()
self.portHandler.setBaudRate(baudrate)
wait(1)
if len(self.ping()) < 6:
raise Exception('Actuators Count Error')

def move_position(self, pos):

pos_mapped = [int(round(map((90 + i), -90, 90, 0, 2048))) for i in pos]
goal_positions = [[DXL_LOBYTE(DXL_LOWORD(i)),
DXL_HIBYTE(DXL_LOWORD(i)),
DXL_LOBYTE(DXL_HIWORD(i)),
DXL_HIBYTE(DXL_HIWORD(i))] for i in pos_mapped]

for i in range(len(goal_positions)):
dxl_addparam_result = self.groupSyncWrite.addParam(i + 1,
goal_positions[i])
if dxl_addparam_result != True:
print('False')
self.groupSyncWrite.txPacket()
self.groupSyncWrite.clearParam()

# Depreciated
def set_moving_speed(self, speed):
for i in range(1, 7):
self.set_goal_speed_single(i, speed[i - 1])

# Depreciated
def set_goal_position_single(self, id, pos):
self.packetHandler.write2ByteTxRx(self.portHandler, id, 30, int(
round(map(((90 - pos) if id % 2 != 0 else (90 + pos)), -90, 90, 0,
2048))))

# Depreciated
def set_goal_speed_single(self, id, speed):
self.packetHandler.write2ByteTxRx(self.portHandler, id, 32, speed)

def set_led_single(self, id, state):

self.packetHandler.write1ByteTxRx(self.portHandler, id, 25, state)

def set_led(self, pattern, state):

for i in pattern:
self.set_led_single(i, state)

def ping(self):
found_ids = []
for i in range(1, 10):
dxl_model_number, dxl_comm_result, dxl_error =
self.packetHandler.ping(self.portHandler, i)
if dxl_comm_result == COMM_SUCCESS:
 found_ids.append(i)
return found_ids

def close(self):
self.portHandler.closePort()

from math import *

from time import sleep
import pypot.dynamixel.conversion
import os
import numpy as np
from tqdm import tqdm

def rad(deg):
return deg / 180.0 * pi

def deg(rad):
return rad * 180.0 / pi

class Robot:
motorH = 102.30
HOME_Z = 295.92
RELATIVE = 'rel'
ABSOLUTE = 'abs'
x, y, z, roll, pitch, yaw = 0, 0, HOME_Z, 0, 0, 0

LINK_1 = 80.0
LINK_2 = 220.0

DEFAULT_LINK_2_CORR = np.array([0, 0, 0, 0, 0, 0])

platform = np.array(
[[23.56, 141.78, 0, 1], [-23.56, 141.78, 0, 1], [-134.57, -50.49, 0, 1], [-
110.77, -91.59, 0, 1],
[110.77, -91.59, 0, 1], [134.57, -50.49, 0, 1]])
base = np.array([[58.66, 144.14, 0, 1], [-58.66, 144.14, 0, 1], [-154.15, -
21.27, 0, 1], [-95.5, -122.86, 0, 1],
[95.5, -122.86, 0, 1], [154.15, -21.27, 1]])

def __init__(self, port=None, LINK_1=80.0, LINK_2=220.0,

link_1_correction=np.array([0, 0, 0, 0, 0, 0]),
link_2_correction=DEFAULT_LINK_2_CORR):
self.actuators = Actuators(port)

self.LINK_1 = LINK_1
self.LINK_2 = LINK_2

self.link_1_correction = link_1_correction
self.link_2_correction = link_2_correction

self.tool_x = self.tool_y = self.tool_z = 0

for i in (1, 2, 3, 4, 5, 6):

for j in (True, False):
self.actuators.set_led([i], j)
 wait(0.07)
self.actuators.set_led([1, 2, 3, 4, 5, 6], True)
wait(0.1)
self.actuators.set_led([1, 2, 3, 4, 5, 6], False)

def close(self):
self.actuators.close()

def reset(self):
self.goto()

def tool(self, x=0, y=0, z=0):

self.tool_x, self.tool_y, self.tool_z = x, y, z

def goto(self, x=0, y=0, z=HOME_Z, roll=0, pitch=0, yaw=0):

self.x, self.y, self.z, self.roll, self.pitch, self.yaw = x, y, z, roll,
pitch, yaw
# Update Geometry (as per as stewart dimensions)
link1 = np.array([self.LINK_1 + self.link_1_correction[i] for i in
range(6)])
link2 = np.array([self.LINK_2 + self.link_2_correction[i] for i in
range(6)])

hs = self.platform
sh = self.base

# Applying transformation
hs = np.matmul(hs, translation(-self.tool_x, -self.tool_y, -self.tool_z))
hs = np.matmul(hs, rotation(rad(self.roll), rad(self.pitch),
rad(self.yaw)))
hs = np.matmul(hs, translation(self.tool_x, self.tool_y, self.tool_z))
hs = np.matmul(hs, translation(self.x, self.y, self.z - self.motorH))

# iKin
l1o = np.array([[hs[0][0] - sh[0][0]], [hs[0][1] - sh[0][1]], [hs[0][2] -
sh[0][2]]])
l2o = np.array([[hs[1][0] - sh[1][0]], [hs[1][1] - sh[1][1]], [hs[1][2] -
sh[1][2]]])
l3o = np.array([[hs[2][0] - sh[2][0]], [hs[2][1] - sh[2][1]], [hs[2][2] -
sh[2][2]]])
l4o = np.array([[hs[3][0] - sh[3][0]], [hs[3][1] - sh[3][1]], [hs[3][2] -
sh[3][2]]])
l5o = np.array([[hs[4][0] - sh[4][0]], [hs[4][1] - sh[4][1]], [hs[4][2] -
sh[4][2]]])
l6o = np.array([[hs[5][0] - sh[5][0]], [hs[5][1] - sh[5][1]], [hs[5][2] -
sh[5][2]]])

l1 = np.matmul(rotation_z(rad(-150)).transpose(), l1o)
l2 = np.matmul(rotation_z(rad(150)).transpose(), l2o)
l3 = np.matmul(rotation_z(rad(-30)).transpose(), l3o)
l4 = np.matmul(rotation_z(rad(-90)).transpose(), l4o)
l5 = np.matmul(rotation_z(rad(90)).transpose(), l5o)
l6 = np.matmul(rotation_z(rad(30)).transpose(), l6o)

thetav1 = asin(l1[0][0] / link2[0])

thetav2 = asin(l2[0][0] / link2[1])
thetav3 = asin(l3[0][0] / link2[2])
thetav4 = asin(l4[0][0] / link2[3])
thetav5 = asin(l5[0][0] / link2[4])
 thetav6 = asin(l6[0][0] / link2[5])

theta21 = acos((l1[1][0] ** 2 + l1[2][0] ** 2 - link1[0] ** 2 - (link2[0] *

cos(thetav1)) ** 2) / (
2 * link1[0] * link2[0] * cos(thetav1)));
theta22 = acos((l2[1][0] ** 2 + l2[2][0] ** 2 - link1[1] ** 2 - (link2[1] *
cos(thetav2)) ** 2) / (
2 * link1[1] * link2[1] * cos(thetav2)));
theta23 = acos((l3[1][0] ** 2 + l3[2][0] ** 2 - link1[2] ** 2 - (link2[2] *
cos(thetav3)) ** 2) / (
2 * link1[2] * link2[2] * cos(thetav3)));
theta24 = acos((l4[1][0] ** 2 + l4[2][0] ** 2 - link1[3] ** 2 - (link2[3] *
cos(thetav4)) ** 2) / (
2 * link1[3] * link2[3] * cos(thetav4)));
theta25 = acos((l5[1][0] ** 2 + l5[2][0] ** 2 - link1[4] ** 2 - (link2[4] *
cos(thetav5)) ** 2) / (
2 * link1[4] * link2[4] * cos(thetav5)));
theta26 = acos((l6[1][0] ** 2 + l6[2][0] ** 2 - link1[5] ** 2 - (link2[5] *
cos(thetav6)) ** 2) / (
2 * link1[5] * link2[5] * cos(thetav6)));

A1 = sqrt(
(link1[0] + link2[0] * cos(thetav1) * cos(theta21)) ** 2 + (link2[0] *
cos(thetav1) * sin(theta21)) ** 2);
A2 = sqrt(
(link1[1] + link2[1] * cos(thetav2) * cos(theta22)) ** 2 + (link2[1] *
cos(thetav2) * sin(theta22)) ** 2);
A3 = sqrt(
(link1[2] + link2[2] * cos(thetav3) * cos(theta23)) ** 2 + (link2[2] *
cos(thetav3) * sin(theta23)) ** 2);
A4 = sqrt(
(link1[3] + link2[3] * cos(thetav4) * cos(theta24)) ** 2 + (link2[3] *
cos(thetav4) * sin(theta24)) ** 2);
A5 = sqrt(
(link1[4] + link2[4] * cos(thetav5) * cos(theta25)) ** 2 + (link2[4] *
cos(thetav5) * sin(theta25)) ** 2);
A6 = sqrt(
(link1[5] + link2[5] * cos(thetav6) * cos(theta26)) ** 2 + (link2[5] *
cos(thetav6) * sin(theta26)) ** 2);

psi1 = atan2((link2[0] * cos(thetav1) * sin(theta21)), (link1[0] + link2[0]

* cos(thetav1) * cos(theta21)));
psi2 = atan2((link2[1] * cos(thetav2) * sin(theta22)), (link1[1] + link2[1]
* cos(thetav2) * cos(theta22)));
psi3 = atan2((link2[2] * cos(thetav3) * sin(theta23)), (link1[2] + link2[2]
* cos(thetav3) * cos(theta23)));
psi4 = atan2((link2[3] * cos(thetav4) * sin(theta24)), (link1[3] + link2[3]
* cos(thetav4) * cos(theta24)));
psi5 = atan2((link2[4] * cos(thetav5) * sin(theta25)), (link1[4] + link2[4]
* cos(thetav5) * cos(theta25)));
psi6 = atan2((link2[5] * cos(thetav6) * sin(theta26)), (link1[5] + link2[5]
* cos(thetav6) * cos(theta26)));

self.theta11 = acos(l1[1][0] / A1) - psi1;

self.theta12 = acos(l2[1][0] / A2) - psi2;
self.theta13 = acos(l3[1][0] / A3) - psi3;
self.theta14 = acos(l4[1][0] / A4) - psi4;
self.theta15 = acos(l5[1][0] / A5) - psi5;
self.theta16 = acos(l6[1][0] / A6) - psi6;
 self.actuators.move_position(
[-deg(self.theta11), deg(self.theta12), -deg(self.theta13),
deg(self.theta14), -deg(self.theta15),
deg(self.theta16)])
return [self.theta11, self.theta12, self.theta13, self.theta14,
self.theta15, self.theta16]

def update(self):
self.goto(self.x, self.y, self.z, self.roll, self.pitch, self.yaw)

def move(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0):

self.goto(self.x+x, self.y+y, self.z+z, self.roll+roll, self.pitch+pitch,
self.yaw+yaw)

def move_relative(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, T=2, steps=100):
dx, dy, dz, droll, dpitch, dyaw = x / steps, y / steps, z / steps, roll /
steps, pitch / steps, yaw / steps
for i in tqdm(range(steps)):
self.move(x=dx, y=dy, z=dz, roll=droll, pitch=dpitch, yaw=dyaw)
wait(T / steps)

def move_absolute(self, x=0, y=0, z=HOME_Z, roll=0, pitch=0, yaw=0, T=2,

steps=100):
self.move_relative(x=(x - self.x),
y=(y - self.y),
z=(z - self.z),
roll=(roll - self.roll),
pitch=(pitch - self.pitch),
yaw=(yaw - self.yaw),
T=T,
steps=steps)