<jupyter_start><jupyter_text>Oppgave 5<jupyter_code>import open3d as o3d
import numpy as np

def Ry_sym(theta):
    ct = sp.cos(theta)
    st = sp.sin(theta)
    R = sp.Matrix([[ct, 0.0, st], [0.0, 1.0, 0.0], [-st, 0, ct]])
    return R

class Robot_object:
    Robot_objects = []
    def __init__(self, num_joints, length_links):
        self.num_joints = num_joints
        self.length_links = length_links
        self.Joints = []
        self.Links = []
        self.make_robot_objects()


    def set_colour(self, colour):
        self.colour = colour

    def make_robot_objects(self):
        for i in range(self.num_joints):
            self.Joints.append(Joint())
        for i in range(len(self.length_links)):
            self.Links.append(Link(self.length_links[i]))
    
    def transform(self, T_list):
        for i, J in enumerate(self.Joints):
            J.transform(T_list[i])
        for i, L in enumerate(self.Links):
            L.transform(T_list[i])

    def draw_robot(self):
        o3d.visualization.draw_geometries(self.Robot_objects)


class Joint(Robot_object):
    colour = [0,1,0]
    def __init__(self):
        self.joint = o3d.geometry.TriangleMesh.create_cylinder(radius=0.1, height=0.3)
        self.coord = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
        self.update_mesh_list()
        self.set_colour(self.colour)

    def update_mesh_list(self):
        self.Robot_objects.append(self.joint)
        self.Robot_objects.append(self.coord)
    
    def set_colour(self, colour):
        self.joint.paint_uniform_color(colour)

    def transform(self,T):
        self.joint = self.joint.transform(T)
        self.coord = self.coord.transform(T)

class Link(Robot_object):
    colour = [1,0,1]
    def __init__(self, lenght):
        self.lenght = lenght
        self.link = o3d.geometry.TriangleMesh.create_cylinder(radius=0.01, height=self.lenght).rotate(Ry_sym(np.pi/2)).translate(np.array([self.lenght/2,0,0]))
        self.update_mesh_list()
        self.set_colour(self.colour)

    def update_mesh_list(self):
        self.Robot_objects.append(self.link)
    
    def set_colour(self, colour):
        self.link.paint_uniform_color(colour)
    
    def transform(self,T):
        self.link = self.link.transform(T)
 
f1 = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)

def make_transform_list(Mi, S_list, thetas):
    T_list = [] # List with T01,T02,T03...
    T = np.eye(4)
    for i in range(len(thetas)):
        T = T @ mr.MatrixExp6(mr.VecTose3(S_list[:, i] * thetas[i])) @ M
    T01 = mr.MatrixExp6(mr.VecTose3(S1*th1)) @ M1
#"Old" M:


M1=sp.Matrix([[0, 1, 0, 0],
             [1, 0, 0, 0],
             [0, 0, -1, 200], 
             [0, 0, 0, 1]])

M2=sp.Matrix([[0, 1, 0, 25],
             [0, 0, 1, 0],
             [1, 0, 0, 400],
             [0, 0, 0, 1]])

M3=sp.Matrix([[1, 0, 0, 25],
             [0, 0, 1, 0],
             [0, -1, 0, 855],
             [0, 0, 0, 1]])
             
M4=sp.Matrix([[0, 0, -1, 25+420],
             [0, 1, 0, 0],
             [1, 0, 0, 400+455+35],
             [0, 0, 0, 1]])
M5=sp.Matrix([[1, 0, 0, 25+420],
             [0, 0, 1, 0],
             [0, -1, 0, 400+455+35],
             [0, 0, 0, 1]])
M6=sp.Matrix([[0, 0, -1, 525], 
             [0, 1, 0, 0],
             [1, 0, 0, 400+455+35],
             [0, 0, 0, 1]])
Mlist = np.array([M1,M2,M3,M4,M5,M6], dtype=float)

om = sp.zeros(3,6)
om1 = om[:, 0] = M1[:3, 2]
om2 = om[:, 1] = M2[:3, 2]
om3 = om[:, 2] = M3[:3, 2]
om4 = om[:, 3] = M4[:3, 2]
om5 = om[:, 4] = M5[:3, 2]
om6 = om[:, 5] = M6[:3, 2]
q = sp.zeros(3,6)
q1 = q[:,0] = M1[:3, 3]
q2 = q[:,1] = M2[:3, 3]
q3 = q[:,2] = M3[:3, 3]
q4 = q[:,3] = M4[:3, 3]
q5 = q[:,4] = M5[:3, 3]
q6 = q[:,5] = M6[:3, 3]

Slist = Slist_maker(om,q)
Slist
# oppg 6

M_poe = Matrix([[1,0,0,490],
                [0,1,0,0],
                [0,0,1,-420],
                [0,0,0,1]])

T_s = (exp6(S_sp[:,0], theta[0]) * exp6(S_sp[:,1], theta[1]) * exp6(S_sp[:,2], theta[2]) * exp6(S_sp[:,3], theta[3]) * exp6(S_sp[:,4], theta[4]) * exp6(S_sp[:,5], theta[5])) * M

T_b = M * (exp6(S_bp[:,0], theta[0]) * exp6(S_bp[:,1], theta[1]) * exp6(S_bp[:,2], theta[2]) * exp6(S_bp[:,3], theta[3]) * exp6(S_bp[:,4], theta[4]) * exp6(S_bp[:,5], theta[5]))

T_s.simplify()
T_b.simplify()


T_s, T_b<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.19552169898430286, 'fraction_numerical': 0.07456140350877193, 'mean_word_length': 2.458100558659218, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '1', 'dir': 'jupyter-structured-clean-dedup', 'id': '486148', 'license': 'Apache-2.0', 'n_tokens_mistral': 2021, 'n_tokens_neox': 1866, 'n_words': 354, 'path': 'Arkiv/task_21_to_24_jupyter.ipynb', 'repo_name': 'BirkHveding/RobotTek', 'size': '4332'}