Python 机器人学 —— 机械臂工作空间分析_机械臂求作业空间代码是自己编的吗

设关节1、关节5的高 (圆柱高) 分别为 ，5个关节的转动角分别为 

可得出DH参数表：

因为把各个关节的坐标系位置都定在了圆柱的中心，所以需要考虑关节1、关节5的高度。其次，关节1在全局坐标系中的位置应为：

根据5个关节的旋转角 + DH参数表，即可得到各个关节的坐标系，利用坐标系信息可完成图像的绘制

拟定义一个类，使其在输入5个关节的旋转角时，可完成绘制工作

环境设置

这次实验主要用到了 numpy、matplotlib，另外还用到了我自己编写的一个模块 coord.py

coord.py 中的 CoordSys_3d 用于描述齐次坐标系，并为图形的仿射变换提供了接口，源代码位于：https://blog.csdn.net/qq_55745968/article/details/129912954

from functools import partial
 
import matplotlib.pyplot as plt
import numpy as np
 
# coord.py 详见: https://blog.csdn.net/qq_55745968/article/details/129912954
from coord import CoordSys_3d
 
rot = CoordSys_3d.rot
trans = CoordSys_3d.trans
 
red = 'orangered'
orange = 'orange'
yellow = 'yellow'
green = 'greenyellow'
cyan = 'aqua'
blue = 'deepskyblue'
purple = 'mediumpurple'
pink = 'violet'
 
ROUND_EDGE = 30  # 圆等效多边形边数
DTYPE = np.float16  # 矩阵使用的数据类型
 
# 三个相同关节的尺寸参数
COMMON_HEIGHT = 20
COMMON_RADIUS_OUT = 10
COMMON_RADIUS_IN = 3
# 连杆外径
CONNECT_ROD_RADIUS = COMMON_RADIUS_OUT / 3
# 各个关节的尺寸参数
JOINTS_SHAPE = [[COMMON_HEIGHT * 0.8, COMMON_RADIUS_IN * 2, COMMON_RADIUS_OUT, 'Wistia_r', 10],
                [COMMON_RADIUS_OUT, COMMON_RADIUS_IN, COMMON_HEIGHT, 'cool', 2],
                [COMMON_RADIUS_OUT, COMMON_RADIUS_IN, COMMON_HEIGHT, 'cool', 2],
                [COMMON_RADIUS_OUT, COMMON_RADIUS_IN, COMMON_HEIGHT, 'cool', 2],
                [None for _ in range(5)],
                [COMMON_RADIUS_OUT * 0.8, COMMON_RADIUS_IN, COMMON_HEIGHT * 0.8, 'cool', 10]]
PACE_NUM = 20  # 转动范围分解步数
THETA = np.linspace(-90, 90, PACE_NUM)
 
 
def DH_PARAM(theta):
    ''' DH 参数表'''
    return [[theta[0], 56 - JOINTS_SHAPE[0][2] / 2, 0, 90],  # 关节 1 -> 关节 2
            [90 + theta[1], 0, 43, 0],  # 关节 2 -> 关节 3
            [theta[2], 0, 43, 0],  # 关节 3 -> 关节 4
            [-90 + theta[3], 0, 0, -90],  # 关节 4 -> 关节 5
            [theta[4], 45.5 - JOINTS_SHAPE[-1][2] / 2, 0, 0]]

绘图函数

def figure3d():
    ''' 创建3d工作站'''
    figure = plt.subplot(projection='3d')
    tuple(getattr(figure, f'set_{i}label')(i) for i in 'xyz')
    return figure
 
 
def cylinder(figure, state: CoordSys_3d,
             R: float, h: float, r: float = 0,
             axis: int = 2, smooth: int = 2):
    ''' 以 state 的 z 轴为主轴绘制圆柱
        figure: 3D 工作站对象
        state: CoordSys_3d 齐次坐标系
        R: 圆柱底面外径
        r: 圆柱底面内径
        h: 圆柱高度
        axis: 圆柱两底面圆心连线所在的轴索引
        smooth: 图像细致程度 (至少 2)'''
    # 当主轴为 x,y 时, 对坐标系进行变换
    if axis < 2:
        rotate = CoordSys_3d.rot(90, 'y' if axis == 0 else 'x')
        state = state.rela_tf(rotate)
    func = []
    theta = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE)
    z = np.linspace(-h / 2, h / 2, smooth, dtype=DTYPE)
    theta, z = np.meshgrid(theta, z)
    # 绘制圆柱内外曲面: 以 z 轴为主轴, 原点为中心
    x, y = np.cos(theta), np.sin(theta)
    func.append(partial(figure.plot_surface, *state.apply(x * R, y * R, z)))
    func.append(partial(figure.plot_surface, *state.apply(x * r, y * r, z)))
 
    phi = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE)
    radius = np.linspace(r, R, 2, dtype=DTYPE)
    phi, radius = np.meshgrid(phi, radius)
    # 绘制上下两底面: 法向量为 z 轴, 原点为中心, 在 z 轴上偏移得到两底面
    x, y = np.cos(phi) * radius, np.sin(phi) * radius
    z = np.zeros_like(x)
    for dz in (-h / 2, h / 2):
        s = state.rela_tf(CoordSys_3d.trans(dz=dz))
        func.append(partial(figure.plot_surface, *s.apply(x, y, z)))
    # 返回函数流的执行函数
    return lambda cmap: tuple(f(cmap=cmap) for f in func)

机械臂对象

每次工作状态改变时 (即机械臂运动)，保存机械臂末端的点坐标，绘制成工作空间。在这个过程中，通过 numpy 判断新的工作点是否与旧的工作点重叠，并只保存不重叠的工作点

class Robot_Arm:
    ''' 机械臂对象'''
    fig = figure3d()
    state = CoordSys_3d().rela_tf(trans(0, 0, JOINTS_SHAPE[0][2] / 2))
 
    def __init__(self):
        self.restart()
 
    def restart(self):
        ''' 重启工作空间记录器'''
        self.work_space = np.ones([0, 3])
 
    def refresh(self, theta=[0] * 5, cla=True, draw_body=True, keep_all=False):
        ''' 根据旋转角度刷新画面
            theta: 机械臂各个关节的旋转角
            cla: 刷新时清空画布
            draw_body: 绘制机械臂
            keep_all: 不筛除重复工作点'''
        joints = self.search(theta)
        if cla:
            plt.cla()
            self.fig.view_init(elev=5, azim=-90)
            # 设置 3D 工作站边界
            for set_ in self.fig.set_xlim3d, self.fig.set_ylim3d:
                set_(-150, 150)
            self.fig.set_zlim3d(-50, 200)
            plt.tight_layout()
 
        if draw_body:
            # 绘制关节及其坐标系
            for (R, r, h, cmap, smooth), joint in zip(JOINTS_SHAPE, joints):
                if R:
                    cylinder(self.fig, joint, R=R, r=r, h=h, axis=2, smooth=smooth)(cmap)
                    joint.plot_coord_sys(linewidth=2, length=30)
            # 绘制连杆
            for axis, length, rear in zip([1, 0, 0, None, 2], [56, 43, 43, 0, 45.5], joints[1:]):
                if length:
                    move = -length / 2
                    connect_rod = rear.rela_tf(trans(*map(lambda i: (axis == i) * move, range(3))))
                    cylinder(self.fig, connect_rod, CONNECT_ROD_RADIUS, length, axis=axis)('winter_r')
 
        tail = joints[-1].rela_tf(trans(0, 0, JOINTS_SHAPE[-1][2] / 2)).position.reshape(1, -1)
        # 检测新工作点是否与旧工作点重叠
        if keep_all or np.all(((tail - self.work_space) ** 2).sum(axis=1) > 18):
            self.work_space = np.append(self.work_space, tail, axis=0)
        if cla:
            self.fig.scatter(*map(lambda i: self.work_space[:, i], range(3)), c=red, alpha=0.4, s=10)
            plt.pause(0.001)
 
    def search(self, theta):
        ''' 搜索关节的位置'''
        joints = [self.state]
        for rot_z, trans_z, trans_x, rot_x in DH_PARAM(theta):
            joints.append(joints[-1].rela_tf(rot(rot_z, 'z')
                                             ).rela_tf(trans(trans_x, 0, trans_z)
                                                       ).rela_tf(rot(rot_x, 'x')))
        return joints

工作空间绘制

def draw_work_space():
    ''' 绘制工作空间剖面轮廓'''
    out_points = []
    in_points = []
    # 绘制外圆上半部分
    for t in THETA:
        theta = [0 for _ in range(5)]
        theta[1] = t
        arm.refresh(theta)
    out_points.append(arm.work_space)
    # 绘制外圆下半部分
    for c in 90, -90:
        arm.restart()
        for t in THETA:
            arm.refresh([0, c, t, 0, 0])
        out_points.append(arm.work_space)
    # 绘制外圆中下部分
    for c in 90, -90:
        arm.restart()
        for t in THETA:
            arm.refresh([0, c, c, t, 0])
        out_points.append(arm.work_space)
    # 绘制内圆
    for c in 90, -90:
        arm.restart()
        for t in THETA:
            arm.refresh([0, t, c, c, 0])
        in_points.append(arm.work_space)
    # 绘制轨迹
    arm.restart()
    arm.refresh()
    for line in out_points + in_points:
        loc = list(map(lambda i: line[:, i], range(3)))
        arm.fig.plot(*loc, c=red, linewidth=2, alpha=0.7)
        arm.fig.scatter(*loc, c=red, alpha=0.4, s=10)
 
 
def draw_point_clound():
    ''' 绘制工作空间点云'''
    for a in THETA:
        for b in THETA:
            for c in THETA:
                for d in THETA:
                    arm.refresh([a, b, c, d, 0], draw_body=False, cla=False, keep_all=True)
    arm.refresh()
 
 
arm = Robot_Arm()
draw_work_space()
plt.show()

最终结果

当5个关节的转动角均为0时，机械臂处在工作原点 (如下图所示)。对于这5个旋转关节而言，其z轴 (粉色轴) 均处在其右手规则旋转的方向上；其x轴 (橙色轴) 均处在其z轴与上一个关节的z轴的公垂线方向上，满足DH表示法的要求

当机械臂的5个转角分别为 [0, , 0, 0, 0] 时，其工作点分布在   的圆内

 当机械臂的5个转角分别为 [0, , -90, -90, 0] 时，可绘制出如下工作点。取  进行计算，半径 

当机械臂的5个转角分别为 [0, -90, , 0, 0] 时，其工作点分布在   的圆内

当机械臂的5个转角分别为 [0, -90, -90, , 0] 时，其工作点分布在   的圆内

 将所有范围叠加，得到工作空间的边界如下：