Python 机器人学 —— 坐标系变换动画

环境设置：

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

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

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

np.set_printoptions(precision=4, suppress=True)

red = 'orangered'
orange = 'orange'
yellow = 'yellow'
blue = 'deepskyblue'
purple = 'mediumpurple'
pink = 'violet'

命令执行：

对于给定的旋转、平移命令，这个函数用于得到在变换过程中的所有齐次坐标系，即齐次坐标系的变换轨迹

def run(commands, start: CoordSys_3d = None):
    ''' 执行运动命令
        rot: ('rot', angle, axis, 'abs'/'rela', pace)
        trans: ('trans', dx, dy, dz, 'abs'/'rela', pace)'''
    coord = [start if start else CoordSys_3d()]
    for i, com in enumerate(commands):
        # 解析参数
        type_, *args, refer_coord, pace = com
        # 分步旋转矩阵
        if type_ == 'rot':
            angle_sum, axis = args
            angle = angle_sum / pace
            tf = CoordSys_3d.rot(angle, axis)
        # 分步平移矩阵
        elif type_ == 'trans':
            move_sum = np.array(args) / pace
            tf = CoordSys_3d.trans(*move_sum)
        else:
            raise AssertionError(f'Command {i + 1}: Illegal parameter <{type_}>')
        # 绝对变换 / 相对变换
        for p in range(pace):
            coord += [getattr(coord[-1], f'{refer_coord}_tf')(tf)]
    return coord

命令分步：

由 旋转步数、平移步数 得到适用于 run 函数的命令组

def get_commands(rot_pace: int, trans_pace: int):
    ''' 旋转步数、平移步数 -> 命令组'''
    return (
        ('rot', 90, 'z', 'abs', rot_pace),
        ('rot', 90, 'y', 'abs', rot_pace),
        ('trans', 4, -3, 7, 'abs', trans_pace)
    )

3d坐标系绘制：

def figure3d():
    ''' 创建3d工作站'''
    figure = plt.subplot(projection='3d')
    tuple(getattr(figure, f'set_{i}label')(i) for i in 'xyz')
    figure.view_init(azim=70)
    return figure


# 全局坐标系 / 齐次坐标系绘制函数
plot_global_sys = lambda: CoordSys_3d().plot_coord_sys(linewidth=5, length=1, colors=[red, pink, purple])
plot_coord_sys = lambda s: s.plot_coord_sys(labels='noa', colors=[orange, yellow, blue])

播放动画/逐步变换：

def show(cartoon=False, track=False):
    # 播放动画
    if cartoon:
        figure3d()
        plot_global_sys()
        plt.pause(3)
        if track:
            # 有轨迹
            for i, s in enumerate(run(get_commands(10, 10))):
                plot_coord_sys(s)
                if i == 0: plt.legend()
                plt.pause(0.05)
        else:
            # 无轨迹
            for i, s in enumerate(run(get_commands(30, 30))):
                plt.cla()
                plot_global_sys()
                plot_coord_sys(s)
                plt.legend()
                plt.pause(0.02)
        plt.pause(0)
    else:
        # 逐步变换
        for i, s in enumerate(run(get_commands(1, 1))):
            figure3d()
            plot_global_sys()
            plot_coord_sys(s)
            plt.legend()
            plt.show()


show(cartoon=True, track=True)

最终效果：