逆运动学和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()
`

dega 数组只是修改正确方向的角度。

我正在尝试使用 matlab 复制我想到的设计。 我已经计算出了数学结果，我需要帮助将其翻译到 matlab 3d 空间中。我得到不同的轴等，它只是将点对齐在一起。 我正在使用

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

存储前一行的终点作为下一行的开始，但它对我来说不起作用。

有人可以帮忙吗？

提前谢谢您。

ps 目标应该很明确，我想使用滑块移动脚端点，并查看生成的角度，这将是伺服角度。所有信息都在有关长度的代码中。

如果能得到任何反馈就很高兴了

Answer 1

试试这个

    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()

'''