逆向运动学与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]

来存储前一条线的结束点，以便作为下一条线的起点，但这对我来说就是不太奏效。

附言：目标应该很明确，我想通过滑块移动脚的结束点，并查看生成的角度，这些角度就是伺服电机的角度。关于长度的所有信息都在代码里。

matlab 坐标系 3d建模逆向运动学伺服电机角度调整动态模拟

逆向运动学与MATLAB

1 个回答

撰写回答