{'subtasks': [{'description': 'Review the existing Python code for the robot to understand its structure

and functionality.', 'tool': None}, {'description': 'Choose a suitable ML model for the task of bone
alignment (e.g., regression model, classification model, or neural network).', 'tool': None},
{'description': 'Gather and preprocess the training data required for the ML model. Split the data
into training and testing sets.', 'tool': None}, {'description': "Implement the code to train the selected
ML model using the training data. Evaluate the model's performance using the testing data.", 'tool':
None}, {'description': 'Modify the existing Python code to include the trained ML model. Ensure that
the model can make predictions based on the input data from the robot.', 'tool': None},
{'description': 'Run tests to ensure that the integrated system works as expected. Validate the
performance of the robot with the integrated ML model.', 'tool': None}]}

All done! What's next on the agenda?

Let's start by reviewing the existing Python code for the robot to understand its structure and
functionality. Please provide the Python code for the robot named "AUTOMATION OF FRACTURED
FEMUR BONE ALIGNMENT USING PARALLEL ROBOT".

Great! Let's proceed with the integration of an ML model into this code. Here are the steps we will
follow:

1. Review the existing Python code for the robot to understand its structure and
functionality. (Done)

2. Choose a suitable ML model for the task of bone alignment.

3. Gather and preprocess the training data required for the ML model. Split the data into
training and testing sets.

4. Implement the code to train the selected ML model using the training data. Evaluate the
model's performance using the testing data.

5. Modify the existing Python code to include the trained ML model. Ensure that the model
can make predictions based on the input data from the robot.

6. Run tests to ensure that the integrated system works as expected. Validate the
performance of the robot with the integrated ML model.

Step 2: Choose a Suitable ML Model

For the task of bone alignment, a regression model would be suitable as we are predicting
continuous values (positions and angles). We can use a neural network regression model for this
purpose.

Step 3: Gather and Preprocess the Training Data

We need training data that includes the input features (e.g., current positions and angles) and the
target outputs (desired positions and angles). For this example, let's assume we have a
dataset bone_alignment_data.csv.
Step 4: Implement the Code to Train the ML Model

Let's start by creating a new Python file train_model.py to implement the training code.

Let's write the code to train a neural network regression model using the training data. We will use
TensorFlow and Keras for this purpose.

The training script has been created successfully. Now, let's install the required packages and run the
training script.

Step 4: Install Required Packages

Final Integrated Code
import joblib
import numpy as np
from math import *
from time import sleep
from dynamixel_sdk import *
import pypot.dynamixel.conversion
from tqdm import tqdm

# Load the trained model and scaler

model = joblib.load('bone_alignment_model.pkl')
scaler = joblib.load('scaler.pkl')

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)
found_ids = self.ping()
print(f"Detected actuator IDs: {found_ids}")
if len(found_ids) < 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()

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()

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, 0, 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)

def predict_and_move(self, current_x, current_y, current_z, current_roll,

current_pitch, current_yaw):
# Prepare the input data
input_data = np.array([[current_x, current_y, current_z, current_roll,
current_pitch, current_yaw]])
input_data = scaler.transform(input_data)

# Make predictions
predictions = model.predict(input_data)
target_x, target_y, target_z, target_roll, target_pitch, target_yaw =
predictions[0]

# Move the robot to the predicted target positions and angles

self.move_absolute(x=target_x, y=target_y, z=target_z, roll=target_roll,
pitch=target_pitch, yaw=target_yaw)

If you have any further questions or need additional modifications, feel free
to ask!
""" 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)
""" 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)