roboter in 3d

Nächster Schritt auf dem Weg zur Optimierung von Kinematiken. Jetzt mit Grafik.

import numpy as np
from scipy.spatial.transform import Rotation
import matplotlib.pyplot as plt
from matplotlib.widgets import Button, Slider


def OpenPlot2D(label_x, label_y,
               xlimMin=None, xlimMax=None,
               ylimMin=None, ylimMax=None,               
               title=None):
    plt.cla()
    
    plt.title(title)
    plt.xlabel(label_x)
    plt.ylabel(label_y)
    
    if ((xlimMin is not None) and ((xlimMax is not None))):
        plt.xlim(xlimMin, xlimMax)
    if ((ylimMin is not None) and ((ylimMax is not None))):
        plt.ylim(ylimMin, ylimMax)
        
    plt.grid(visible = True)


def OpenPlot3D(label_x='x', label_y='y', label_z='z',
               limits=[None, None, None, None, None, None],
               title=None):
    plt.figure(title).add_subplot(projection='3d', proj_type='ortho', aspect='equal')
    plt.cla()
    
    plt.xlabel(label_x)
    plt.ylabel(label_y)
    plt.gca().set_zlabel(label_z)
    
    plt.grid(visible = True)
    if ((limits[0] is not None) and ((limits[1] is not None))):
        plt.xlim(limits[0], limits[1])
        
    if ((limits[2] is not None) and ((limits[3] is not None))):
        plt.ylim(limits[2], limits[3])
        
    if ((limits[4] is not None) and ((limits[5] is not None))):
        plt.gca().set_zlim(limits[4], limits[5])
        
    plt.grid(visible = True)
    plt.gca().set_aspect('equal')


def Vec3(arr):
    v = np.array(arr)
    v = np.append(v, [1])
    return v[:, np.newaxis]


def Trafo3(t, r: Rotation):
    T = np.eye(4)
    T[0:3, 3] = t
    if (type(r) == Rotation):        
        T[0:3, 0:3] = r.as_matrix()
    else:
        T[0:3, 0:3] = Rotation.from_euler('XYZ', r, degrees=True).as_matrix()
    return T
    

def PlotTrafo3(T=np.eye(4), cosLineWidth=1, cosLength=1, name=None, nameSize=10, nameColor='k', draw='xyz'):
    p0 = T[0:3, 3]
    px = p0 + T[0:3, 0]*cosLength
    py = p0 + T[0:3, 1]*cosLength
    pz = p0 + T[0:3, 2]*cosLength
    
    if (draw.find('x') >= 0):
        plt.plot([p0[0], px[0]], [p0[1], px[1]], [p0[2], px[2]], 'r-o', linewidth=cosLineWidth)
    
    if (draw.find('y') >= 0):
        plt.plot([p0[0], py[0]], [p0[1], py[1]], [p0[2], py[2]], 'g-o', linewidth=cosLineWidth)
    
    if (draw.find('z') >= 0):
        plt.plot([p0[0], pz[0]], [p0[1], pz[1]], [p0[2], pz[2]], 'b-o', linewidth=cosLineWidth)
        
    if (name is not None):
        plt.gca().text(px[0], px[1], px[2], name, size=nameSize, color=nameColor)


class Robot:
    T: dict
    T_W_TCP: np.array   # from World to TCP
    
    def __init__(self, T_W_B):
        self.T = dict()
        self.T['T_W_B'] = T_W_B
        self.T_B_TCP = np.eye(4)
    
    def FKin(self, q):        
        # Trafos vom Typ joint aktualisieren (J.xxx.rotZ oder J.xxx.transZ)
        for key in self.T:
            words = key.split('.')
            if (words[0] == 'J'):
                jointNumber = int(words[1])-1
                if (words[2] == 'rotZ'):
                    Tz = Trafo3([0, 0, 0], [0, 0, q[jointNumber]])
                elif (words[2] == 'transZ'):
                    Tz = Trafo3([0, 0, q[jointNumber]], [0, 0, 0])
                else:
                    raise RuntimeError(f'Joint-Type {words[2]} unknown.')
                self.T[key] = Tz                
        
        # Jetzt alle Trafos nacheinander durchführen
        self.T_W_TCP = np.linalg.multi_dot(self.T.values())
        
    def Draw(self, ax=None, cosLength=200):
        if (ax is not None):
            plt.sca(ax)
            
        Tbefore = np.eye(4)
        Tnow = np.eye(4)
        for key in self.T:
            Tbefore = Tnow
            Tnow = Tnow @ self.T[key]
            if (key.startswith('J')):
                PlotTrafo3(Tnow, cosLength=cosLength, name=key[key.find('.')+1:key.rfind('.')] + "'", draw='x')
            else:
                if (key[-3:] == 'TCP'):
                    PlotTrafo3(Tnow, cosLength=cosLength, name=key[key.rfind('_')+1:], draw='xyz')
                else:
                    PlotTrafo3(Tnow, cosLength=cosLength, name=key[key.rfind('_')+1:], draw='xz')
            if (key != 'T_W_B'):
                plt.plot([Tbefore[0, 3], Tnow[0, 3]],
                     [Tbefore[1, 3], Tnow[1, 3]],
                     [Tbefore[2, 3], Tnow[2, 3]],
                     'k-o',
                     linewidth=1)
        
        
class RobotSPI2Joints(Robot):
    def __init__(self, T_W_B):
        Robot.__init__(self, T_W_B)
        self.T['T_B_1'] = Trafo3([0, 0, 200], [0, 0, 0])
        self.T['J.1.rotZ'] = np.eye(4)
        self.T['T_1_2'] = Trafo3([200, 0, 100], [90, 0, 0])
        self.T['J.2.rotZ'] = np.eye(4)
        self.T['T_2_3'] = Trafo3([0, 600, 0], [0, 0, 0])
        self.T['J.3.rotZ'] = np.eye(4)
        self.T['T_3_4'] = Trafo3([0, 300, 0], [-90, 0, 0])
        self.T['J.4.rotZ'] = np.eye(4)
        self.T['T_4_5'] = Trafo3([0, 0, 50], [90, 0, 0])
        self.T['J.5.rotZ'] = np.eye(4)
        self.T['T_5_6'] = Trafo3([0, 50, 0], [-90, 0, 0])
        self.T['J.6.rotZ'] = np.eye(4)
        self.T['T_6_TCP'] = Trafo3([0, 0, 150], [0, 0, 0])
        
r1 = RobotSPI2Joints(Trafo3([0, 0, 0], [0, 0, 0]))
r1.FKin([0, -45, -90, -15, +45, -15])

#r2 = RobotSPI2Joints(Trafo3([200, 200, 0], [0, 0, 0]))
#r2.FKin([-25, 30])

plt.clf()
OpenPlot3D('x', 'y', 'z', limits=[-1000, +2000, -1500, +1500, 0, 2000], title='Robot') 
r1.Draw(cosLength=100)
#r2.Draw()

plt.gca().set_aspect('equal')
plt.gca().view_init(elev=0, azim=-90)
plt.show()


# import time
#
# tStart = time.time()
# f = 0
# for i in range(0, 100000):
#     Trafo3([100, 200, 300], [1.2, 45.9, -183.7]) @ Trafo3([100, 200, 300], [1.2, 45.9, -183.7])
# tEnd = time.time()
# print(tEnd- tStart)