inverse kinematics and matlab

Question

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.widgets import Slider, RadioButtons
import numpy as np
import math


def calculate_line(length, angle, axis):
    # Convert angle to radians
    angle_rad = np.radians(angle)
    # Calculate coordinates of the line based on the selected axis
    if axis == 'XY':
        x = [0, length * np.cos(angle_rad)]
        y = [0, length * np.sin(angle_rad)]
        z = [0, 0]
    elif axis == 'XZ':
        x = [0, length * np.cos(angle_rad)]
        y = [0, 0]
        z = [0, length * np.sin(angle_rad)]
    elif axis == 'YZ':
        x = [0, 0]
        y = [0, length * np.cos(angle_rad)]
        z = [0, length * np.sin(angle_rad)]
    return x, y, z


coax =35
femur = 120
tibia = 140

def update(val):
    rz= rh_slider.val
    
    fz=fz_slider.val
    fy=fy_slider.val
    fx=fx_slider.val
    b1=fy
    b2=coax
    a1=rz
    
    ax.clear()
    ax.plot([0, 0], [0,0], [0, rz ], color='green', label='')
    ax.plot([0, 300], [0,0], [rz, rz ], color='green', label='robot length')
    ax.plot([300, 300], [0,0], [0, rz ], color='green', label='')
    ax.plot([0, 0], [0,150], [rz, rz ], color='green', label='robot width')
    ax.plot([0, 0], [0,150], [rz, rz ], color='green', label='')
    ax.plot([0, 300], [150,150], [rz, rz ], color='green', label='')
    ax.plot([300, 300], [150,0], [rz, rz ], color='green', label='')
    
    print(f'a1 is {a1}')
    c=math.sqrt(pow(a1,2)+pow(fy,2))
    print(f'c is {c}')
    A1=math.degrees(math.asin(a1/c))
    print(f'A1 is {A1}')
    B1=math.degrees(math.asin(b1/c))
    print(f'B1 is {B1}')
    # Top triangle
    a2=math.sqrt(pow(c,2)-pow(b2,2))
    print(f'a2 is {a2}')
    A2=math.degrees(math.asin(a2/c))
    print(f'A2 is {A2}')
    B2=math.degrees(math.asin(b2/c))
    print(f'B2 is {B2}')
    coaxra=int(B1+A2)
    print(f'B1+A2 is {coaxra}')
    z=a1
    if a1 > femur+tibia :
        exit
    x, y, z = calculate_line(coax, dega[coaxra], "YZ")
    ax.plot([0,x[1]], [0,y[1]], [rz,z[1]+rz], color='green', label='coax')
    lx=x[1]
    ly=y[1]+coax
    lz=z[1]
    fa=coaxra+90
    print(f'fa is {fa}')
    x, y, z = calculate_line(a2, dega[fa], "YZ")
    ax.plot([lx,x[1]], [ly,y[1]+fy], [lz,z[1]], color='blue', label='Femur + tibia')
    ax.set_xlim(-400, 400)
    ax.set_ylim(-400, 400)
    ax.set_zlim(-100, 400)
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')




fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

dega=[]
cc=270
for i in range(361):
        dega.append(cc)
        cc=cc-1
        if cc<1:
          cc=360

fz_slider = Slider(plt.axes([0.1, 0.08, 0.65, 0.03]), 'fZ', -200, 200, valinit=0)
fy_slider = Slider(plt.axes([0.1, 0.04, 0.65, 0.03]), 'fY', -200, 200, valinit=coax)
fx_slider = Slider(plt.axes([0.1, 0.12, 0.65, 0.03]), 'fX', -200, 200, valinit=0)
rh_slider = Slider(plt.axes([0.1, 0.15, 0.65, 0.03]), 'RH', 0, 250, valinit=150)

fz_slider.on_changed(update)
fy_slider.on_changed(update)
fx_slider.on_changed(update)
rh_slider.on_changed(update)


plt.show()

The dega array just amends the angle for the right orientation.

I'm attempting to replicate a design i have in mind using matlab. I have the math worked out I need help with translating it within matlab 3d space. I get the different axis etc, it's just aligning the points together.

I'm using:

    lx=x[1]
    ly=y[1]+coax
    lz=z[1]

to store the previous lines end points for the start of the next line but it's just not working out for me.

PS: the aim should be clear I want to move the foot endpoints using the sliders and see the angles that are generated which will be the servo angles. All the info is in the code regarding lengths.

Answer 1

try this

    import matplotlib.pyplot as plt
    from mpl_toolkits.mplot3d import Axes3D
    from matplotlib.widgets import Slider
    import numpy as np
    import math

# Function to calculate coordinates of a line segment
def calculate_line(length, angle, axis):
    angle_rad = np.radians(angle)
    if axis == 'XY':
        x = [0, length * np.cos(angle_rad)]
        y = [0, length * np.sin(angle_rad)]
        z = [0, 0]
    elif axis == 'XZ':
        x = [0, length * np.cos(angle_rad)]
        y = [0, 0]
        z = [0, length * np.sin(angle_rad)]
    elif axis == 'YZ':
        x = [0, 0]
        y = [0, length * np.cos(angle_rad)]
        z = [0, length * np.sin(angle_rad)]
    return x, y, z

# Function to update the plot
def update(val):
    # Get slider values
    rz = rh_slider.val
    fz = fz_slider.val
    fy = fy_slider.val
    fx = fx_slider.val

    # Clear previous plot
    ax.clear()

    # Plot the base of the robot
    ax.plot([0, 0], [0, 0], [0, rz], color='green')
    ax.plot([0, 300], [0, 0], [rz, rz], color='green')
    ax.plot([300, 300], [0, 0], [0, rz], color='green')
    ax.plot([0, 0], [0, 150], [rz, rz], color='green')
    ax.plot([0, 0], [0, 150], [rz, rz], color='green')
    ax.plot([0, 300], [150, 150], [rz, rz], color='green')
    ax.plot([300, 300], [150, 0], [rz, rz], color='green')

    # Calculate angles and lengths for leg segments
    # Note: You may need to adjust the calculations based on your design
    a1 = rz
    c = math.sqrt(pow(a1, 2) + pow(fy, 2))
    A1 = math.degrees(math.asin(a1 / c))
    B1 = math.degrees(math.asin(fy / c))
    a2 = math.sqrt(pow(c, 2) - pow(coax, 2))
    A2 = math.degrees(math.asin(a2 / c))
    B2 = math.degrees(math.asin(coax / c))
    coaxra = int(B1 + A2)

    # Plot leg segments
    x, y, z = calculate_line(coax, dega[coaxra], "YZ")
    ax.plot(x, y, z + rz, color='green')  # Coax
    lx, ly, lz = x[1], y[1] + coax, z[1]  # End point of coax
    fa = coaxra + 90
    x, y, z = calculate_line(a2, dega[fa], "YZ")
    ax.plot([lx, lx + x[1]], [ly, ly + y[1]], [lz, lz + z[1]], 
color='blue')  # Femur + Tibia

    # Set plot limits and labels
    ax.set_xlim(-400, 400)
    ax.set_ylim(-400, 400)
    ax.set_zlim(-100, 400)
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    plt.draw()

# Set up the figure and axes
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

# Generate array of degrees
dega = np.arange(270, -90, -1)

# Create sliders
fz_slider = Slider(plt.axes([0.1, 0.08, 0.65, 0.03]), 'fZ', -200, 200, 
valinit=0)
fy_slider = Slider(plt.axes([0.1, 0.04, 0.65, 0.03]), 'fY', -200, 200, 
valinit=coax)
fx_slider = Slider(plt.axes([0.1, 0.12, 0.65, 0.03]), 'fX', -200, 200, 
valinit=0)
rh_slider = Slider(plt.axes([0.1, 0.15, 0.65, 0.03]), 'RH', 0, 250, 
valinit=150)

# Attach update function to sliders
fz_slider.on_changed(update)
fy_slider.on_changed(update)
fx_slider.on_changed(update)
rh_slider.on_changed(update)

plt.show()

'''