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_3dnp.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 = argsangle = angle_sum / pacetf = CoordSys_3d.rot(angle, axis)# 分步平移矩阵elif type_ == 'trans':move_sum = np.array(args) / pacetf = 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)

最终效果：