!pip install trimesh scipy

Collecting trimesh
  Downloading trimesh-4.9.0-py3-none-any.whl.metadata (18 kB)
Collecting scipy
  Using cached scipy-1.15.3-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl.metadata (61 kB)
Requirement already satisfied: numpy>=1.20 in /home/vdhiman/.local/venvs/ece490/lib/python3.10/site-packages (from trimesh) (2.2.6)
Downloading trimesh-4.9.0-py3-none-any.whl (736 kB)
   ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 736.5/736.5 kB 18.9 MB/s  0:00:00
Using cached scipy-1.15.3-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl (37.7 MB)
Installing collected packages: trimesh, scipy
   ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 2/2 [scipy]━━━━━ 1/2 [scipy]
Successfully installed scipy-1.15.3 trimesh-4.9.0

import trimesh
import numpy as np

Denavit Hartenberg Parameters¶

Compute the transform from DH parameters to the transform matrix.

def dh_param_transform(d, theta, r, alpha):
    """
    Compute the 4x4 transformation matrix associated with the Denavit hartenberg parameters
    """
    T_d_theta = np.array([
        [np.cos(theta), -np.sin(theta), 0, 0],
        [np.sin(theta),  np.cos(theta), 0, 0],
        [            0,              0, 1, d],
        [            0,              0, 0, 1],
    ])
    T_r_alpha = np.array([
        [1,             0,              0, r],
        [0, np.cos(alpha), -np.sin(alpha), 0],
        [0, np.sin(alpha),  np.cos(alpha), 0],
        [0,             0,              0, 1],
    ])
    
    return T_d_theta @ T_r_alpha

Load the meshes at origin¶

I have transformed the meshes so that the origin and coordinate frame is at the correct pose on each mesh. Z-axis (blue) passes through the axis of rotation and X-axis is the mutual perpendicular of the two Z-axis.

Here we visualize each part at their chosen coordinate frame; without any additional transformations.

# Got them from here: https://www.thingiverse.com/thing:2747086
URL_FORMAT = "https://github.com/wecacuee/ECE417-F24-Mobile-Robots/blob/master/docs/notebooks/11-05-fwd-kinematics/meshes/reset/{part_name}_reset.stl?raw=true"
# URL_FORMAT = "meshes/reset/{part_name}_reset.stl"
base1_reset2 = trimesh.load_remote(URL_FORMAT.format(part_name="base1"))
xyz = trimesh.creation.axis(axis_length=200, axis_radius=1) # X = red, Y = green, Z = blue
trimesh.scene.Scene((base1_reset2, xyz)).show()

base2_reset2 = trimesh.load_remote(URL_FORMAT.format(part_name="base2"))
trimesh.scene.Scene((base2_reset2, xyz)).show()

shoulder_reset2 = trimesh.load_remote(URL_FORMAT.format(part_name="shoulder"))
trimesh.scene.Scene((shoulder_reset2, xyz)).show()

elbow_reset2 = trimesh.load_remote(URL_FORMAT.format(part_name="elbow"))
trimesh.scene.Scene((elbow_reset2, xyz)).show()

gripper1_reset2 = trimesh.load_remote(URL_FORMAT.format(part_name="gripper1"))
trimesh.scene.Scene((gripper1_reset2, xyz)).show()

The last link has an arbitrary coordinate frame.

gripper2_reset2 = trimesh.load_remote(URL_FORMAT.format(part_name="gripper2"))
trimesh.scene.Scene((gripper2_reset2, xyz)).show()

links = [base1_reset2, base2_reset2, shoulder_reset2, 
         elbow_reset2, gripper1_reset2, gripper2_reset2]

trimesh.scene.Scene(links).show()

Define Denavit Hartenberg Parameters for the arm¶

import numpy as np
dh_param_table_scaled = np.array([
    [257, +0, -10, np.pi/2],
    [0, np.pi/2, 200, 0],
    [0, -0, 190, 0],
    [3, np.pi/2, 0, np.pi/2],
    [0, 0, 0, 0],
])

def forward_kinematics(thetas, dh_param_table_scaled):
    """
    Compute the transformation matrices for all the links
    """
    # T_0_from_i is a list that contains 4x4 Transformation matrices
    # for example T_0_from_3 converts from 3rd coordinate frame to the zeroth
    T_0_from_i = [np.eye(4)]*(len(dh_param_table_scaled)+1)

    # T_0_from_0 is identity
    T_0_from_i[0] = np.eye(4)
    
    # Use DH Parameters and additional thetas to compute each transform
    for i in range(1, 1+dh_param_table_scaled.shape[0]):
        # Add additional theta for motor rotations
        d, theta, r, alpha = dh_param_table_scaled[i-1]
        theta += thetas[i-1]

        # Compute transformation matrix from i to i-1
        # T_im1_from_i is short for T_(i minus 1) from i
        T_im1_from_i = dh_param_transform(d, theta, r, alpha)

        # Use the previous transformation to get the next one.
        T_0_from_i[i] = T_0_from_i[i-1] @ T_im1_from_i

    # Return the full list
    return T_0_from_i

Now we can use forward kinematics to move the arm

# Change these motor angles to move the robotic arm
motor_angles = [0, 0., 0., 0., 0.]
import ipywidgets as widgets
sliders = [widgets.FloatSlider(min=-180, value=0, max=180) for _ in range(5)]
display(*sliders)

motor_angles = np.deg2rad([ma.value for ma in sliders])
motor_angles

array([ 1.00705498, -1.36310215, 2.04552588, -0.88837259, 1.48178453])

T_0_from_i = forward_kinematics(motor_angles, 
                                dh_param_table_scaled)

transformed_parts = []
xyzs = []
for i in range(len(links)):
    # Apply transform to each mesh part
    transformed_parti = links[i].copy().apply_transform(T_0_from_i[i])
    transformed_parts.append(transformed_parti)

    # Also create transformed coordinate axes for visualization
    xyzs.append(
        trimesh.creation.axis(transform=T_0_from_i[i],
                                  axis_length=200, axis_radius=1)
    )

scene = trimesh.scene.Scene(transformed_parts+xyzs)
scene.show()