sympyにてDHパラメータを記述してみた

DHパラメータとは

DHパラメータは、ロボットアームの関節を4つの値で表現します。a、α、
d、θです。これらの値は、アームの動きや挙動を制御するための変換行列を作成するために使用されます。精密で効率的なロボットアームを設計するために欠かせないツールです。

参考:https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters

1. $${x_{n-1}}$$ 軸周りに$${\alpha_{n-1}}$$だけ回転

2. $${x_{n-1}}$$軸向きに$${a_{n-1}}$$だけ平行移動

3. $${z_{n}}$$軸周りに$${\theta_{n}}$$だけ回転

4. $${z_{n}}$$軸周りに$${d_{n}}$$だけ平行移動

DHパラメータの行列

Pythonにて確認

import sympy
import numpy as np
from sympy import sin,cos,Matrix

def Rz(theta):
    M = Matrix([
        [cos(theta), -sin(theta),0,0],
        [sin(theta),cos(theta),0,0],
        [0,0,1,0],
        [0,0,0,1]
    ])
    return M

def Tz(d):
    M = Matrix([
        [1,0,0,0],
        [0,1,0,0],
        [0,0,1,d],
        [0,0,0,1]
    ])
    return M

def Rx(theta):
    M = Matrix([
        [1,0,0,0],
        [0,cos(theta),-sin(theta),0],
        [0, sin(theta),cos(theta),0],
        [0,0,0,1]
        ])
    return M 

def Tx(d):
    M = Matrix([
        [1,0,0,d],
        [0,1,0,0],
        [0,0,1,0],
        [0,0,0,1]
    ])
    return M

alpha,theta = sympy.symbols('alpha_n-1, theta_n')
a,d = sympy.symbols('a_n-1, d_n')
T01 = Rx(alpha)*Tx(a)*Rz(theta)*Tz(d)
T01

run

実行結果