目录
机器人雅克比的推导是不难,但是比较繁琐,如果已经有了正运动学,我们也可以直接用数值微分的方法求解雅克比
求出来的结果和初始计算T_tar的并不相同,这是因为机器人IK存在多个解,求出来的解是与提供的joints_init比较接近的一组关节值
DH法计算机器人的正运动学
import numpy as np
from functools import reduce
np.set_printoptions(precision=4, suppress=True)
定义绕x/y/z
旋转的旋转矩阵
def rotate(axis, deg):
AXIS = ('X', 'Y', 'Z')
axis = str(axis).upper()
if axis not in AXIS:
print(f"{axis} is unknown axis, should be one of {AXIS}")
return
rot_x = axis == 'X'
rot_y = axis == 'Y'
rot_z = axis == 'Z'
rot_mat = np.array([[(np.cos(deg), 1)[rot_x], (0, -np.sin(deg))[rot_z], (0