机械臂工作空间求解（python、可旋转视图）_解析法求解工作空间 python

使用蒙特卡洛方法计算工作空间

import math
import numpy as np
import plotly.graph_objects as go


# 定义转换矩阵函数
def transformation_matrix(theta):
    a = theta[0]
    b = theta[1]
    c = theta[2]
    # 转换矩阵
    px = (3 * math.cos(c) + 2) * math.cos(a) * math.cos(b) - 3 * math.cos(a) * math.sin(b) * math.sin(c) - 2 * math.sin(
        a)
    py = (3 * math.cos(c) + 2) * math.sin(a) * math.cos(b) - 3 * math.sin(a) * math.sin(b) * math.sin(c) + 2 * math.cos(
        a)
    pz = -1 - 3 * (math.sin(b) * math.cos(c) + math.cos(b) * math.sin(c)) - 2 * math.sin(b)
    # 返回转换矩阵
    return px, py, pz


# 使用蒙特卡洛方法计算工作空间
workspace = []

for _ in range(2000):  # 这里我们使用2000个样本点
    theta = [np.random.uniform(-np.pi, np.pi) for _ in range(3)]  # 对每个关节随机生成角度
    pos = transformation_matrix(theta)
    workspace.append(pos)

workspace = np.array(workspace)

# 绘制工作空间
# 使用你的数据
x, y, z = workspace[:, 0], workspace[:, 1], workspace[:, 2]

# 计算x, y, z方向的最小值和最大值
x_min, x_max = np.min(x), np.max(x)
y_min, y_max = np.min(y), np.max(y)
z_min, z_max = np.min(z), np.max(z)

# 创建一个3D散点图
scatter = go.Scatter3d(
    x=x,
    y=y,
    z=z,
    mode='markers',
    marker=dict(
        size=6,
        color=z,                # 设置颜色为z轴的值
        colorscale='Viridis',    # 选择一种颜色映射
        opacity=0.8
    )
)

# # 创建半透明的3D平面
# planes = [
#     go.Surface(x=[[x_min, x_max], [x_min, x_max]], y=[[y_min, y_min], [y_max, y_max]], z=[[z_min, z_min], [z_min, z_min]], showscale=False, opacity=0.2, colorscale=[(0, 'blue'), (1, 'blue')]),
#     go.Surface(x=[[x_min, x_max], [x_min, x_max]], y=[[y_min, y_min], [y_max, y_max]], z=[[z_max, z_max], [z_max, z_max]], showscale=False, opacity=0.2, colorscale=[(0, 'blue'), (1, 'blue')]),
#     go.Surface(y=[[y_min, y_max], [y_min, y_max]], z=[[z_min, z_min], [z_max, z_max]], x=[[x_min, x_min], [x_min, x_min]], showscale=False, opacity=0.2, colorscale=[(0, 'blue'), (1, 'blue')]),
#     go.Surface(y=[[y_min, y_max], [y_min, y_max]], z=[[z_min, z_min], [z_max, z_max]], x=[[x_max, x_max], [x_max, x_max]], showscale=False, opacity=0.2, colorscale=[(0, 'blue'), (1, 'blue')]),
#     go.Surface(z=[[z_min, z_max], [z_min, z_max]], x=[[x_min, x_min], [x_max, x_max]], y=[[y_min, y_min], [y_min, y_min]], showscale=False, opacity=0.2, colorscale=[(0, 'blue'), (1, 'blue')]),
#     go.Surface(z=[[z_min, z_max], [z_min, z_max]], x=[[x_min, x_min], [x_max, x_max]], y=[[y_max, y_max], [y_max, y_max]], showscale=False, opacity=0.2, colorscale=[(0, 'blue'), (1, 'blue')])
# ]
#
# 添加到图中
fig = go.Figure(data=[scatter])
# fig = go.Figure(data=[scatter] + planes)
#
# # 添加注释
# annotations = [
#     dict(showarrow=False, x=x_min, y=0, z=0, text=f"x={x_min}", font=dict(color='red')),
#     dict(showarrow=False, x=0, y=y_max, z=0, text=f"y={y_max}", font=dict(color='blue'))
# ]
# fig.update_layout(scene=dict(annotations=annotations))

# 设置布局
fig.update_layout(margin=dict(l=0, r=0, b=0, t=0))

fig.show()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78

视图可以拖动

这里求出来的是无视连杆等障碍的工作空间分布。