Scribd
Upload
15 views2 pages

Robot Kinematics with NumPy

This document contains Python code to calculate the forward kinematics of a 3-link robot arm. It takes user input for the link lengths and joint angles, defines a parametric table to represent the kinematic structure, and uses homogeneous transformation matrices to determine the end effector position and orientation (H0_3 matrix) given the joint configurations. Key variables like link lengths, joint angles, and transformation matrices are printed.

Uploaded by

jacksonlachi
Copyright
© All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
15 views2 pages

Robot Kinematics with NumPy

This document contains Python code to calculate the forward kinematics of a 3-link robot arm. It takes user input for the link lengths and joint angles, defines a parametric table to represent the kinematic structure, and uses homogeneous transformation matrices to determine the end effector position and orientation (H0_3 matrix) given the joint configurations. Key variables like link lengths, joint angles, and transformation matrices are printed.

Uploaded by

jacksonlachi
Copyright
© All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd

import numpy as np

# Link length in mm
a1 = float(input("a1="))
a2 = float(input("a2="))
a3 = float(input("a3="))

# Joint variables
d3 = float(input("d3="))
T1 = float(input("T1="))
T2 = float(input("T2="))

# Degrees to radians
T1 = (T1 / 180.0) * [Link]
T2 = (T2 / 180.0) * [Link]

# Parametric table (theta, alpha, r, d)


PT = [
[T1, (90 / 180.0) * [Link], 0, a1],
[T2 + ((90 / 180.0) * [Link]), 0, 0, a2 + a3 + d3],
[0, (0 / 180.0) * [Link], 0, 0]
]

# HTM formulae
H0_1 = [Link]([
[[Link](PT[0][0]), -[Link](PT[0][0]) * [Link](PT[0]
[1]), [Link](PT[0][0]) * [Link](PT[0][1]), PT[0][2] *
[Link](PT[0][0])],
[[Link](PT[0][0]), [Link](PT[0][0]) * [Link](PT[0][1]),
-[Link](PT[0][0]) * [Link](PT[0][1]), PT[0][2] *
[Link](PT[0][0])],
[0, [Link](PT[0][1]), [Link](PT[0][1]), PT[0][3]],
[0, 0, 0, 1]
])

H1_2 = [Link]([
[[Link](PT[1][0]), -[Link](PT[1][0]) * [Link](PT[1]
[1]), [Link](PT[1][0]) * [Link](PT[1][1]), PT[1][2] *
[Link](PT[1][0])],
[[Link](PT[1][0]), [Link](PT[1][0]) * [Link](PT[1][1]),
-[Link](PT[1][0]) * [Link](PT[1][1]), PT[1][2] *
[Link](PT[1][0])],
[0, [Link](PT[1][1]), [Link](PT[1][1]), PT[1][3]],
[0, 0, 0, 1]
])

H2_3 = [Link]([
[[Link](PT[2][0]), -[Link](PT[2][0]) * [Link](PT[2]
[1]), [Link](PT[2][0]) * [Link](PT[2][1]), PT[2][2] *
[Link](PT[2][0])],
[[Link](PT[2][0]), [Link](PT[2][0]) * [Link](PT[2][1]),
-[Link](PT[2][0]) * [Link](PT[2][1]), PT[2][2] *
[Link](PT[2][0])],
[0, [Link](PT[2][1]), [Link](PT[2][1]), PT[2][3]],
[0, 0, 0, 1]
])

H0_1 = [Link](H0_1)
print("H0_1=")
print(H0_1)

H1_2 = [Link](H1_2)
print("H1_2=")
print(H1_2)

H2_3 = [Link](H2_3)
print("H2_3=")
print(H2_3)

H0_2 = [Link](H0_1, H1_2)


H0_3 = [Link](H0_2, H2_3)
print("H0_3=")
print(H0_3)
print([Link]([Link](H0_3, 3)))

You might also like