机器人雅克比Jacobian矩阵程序

% 定义机器人的连杆参数
L1 = Link('d', 0, 'a', 0, 'alpha', pi/2);
L2 = Link('d', 0, 'a', 1, 'alpha', 0);
L3 = Link('d', 0, 'a', 1, 'alpha', 0);% 创建机器人对象
robot = SerialLink([L1, L2, L3], 'name', 'MyRobot');% 设置机器人的关节角度（弧度）
q = [0, pi/4, pi/6];% 计算机器人末端在基坐标系下的雅克比矩阵
J0 = robot.jacob0(q);% 计算机器人末端在自身坐标系下的雅克比矩阵
Jn = robot.jacobn(q);% 显示结果
disp('机器人末端在基坐标系下的雅克比矩阵:');
disp(J0);
disp('机器人末端在自身坐标系下的雅克比矩阵:');
disp(Jn);

参考文献：

https://zhuanlan.zhihu.com/p/2106892917https://zhuanlan.zhihu.com/p/2106892917

L(1) = Revolute('d', 0.15185, 'a', 0, 'alpha', pi/2, 'offset', 0);
L(2) = Revolute('d', 0, 'a', -0.24355, 'alpha', 0, 'offset', 0);
L(3) = Revolute('d', 0, 'a', -0.2132, 'alpha', 0, 'offset', 0);
L(4) = Revolute('d', 0.13105, 'a', 0, 'alpha', pi/2, 'offset', 0);
L(5) = Revolute('d', 0.08535, 'a', 0, 'alpha', -pi/2, 'offset', 0);
L(6) = Revolute('d', 0.0921, 'a', 0, 'alpha', 0, 'offset', 0);
ur3e = SerialLink(L, 'name', 'UR3e');
% 设定一个末端的位置与姿态
TA = SE3(-0.4, -0.2, 0.1) * SE3.Rx(pi/2) * SE3.Ry(-pi/2);
qA = ur3e.ikine(TA);
% 用jacov0计算qA位形下机械臂的雅可比矩阵在基座标系下的描述
JA0 = ur3e.jacob0(qA)
% 用jacove计算qA位形下机械臂的雅可比矩阵在末端工具座标系下的描述
JAE = ur3e.jacobe(qA)
% 验证一下JA0与JAE的关系
RA = ur3e.fkine(qA).R;
RJ = [RA zeros(3,3); zeros(3,3) RA];
% 根据理论公式，JA0 = RJ * JAE
JA0_from_JAE = RJ*JAE

def Jacobian_Matrix():a = np.array([0, -0.425, -0.39225, 0, 0, 0])d = np.array([0.1625, 0, 0, 0.1333, 0.0997, 0.0996 + global_variables.l_k])alpha = np.array([pi/2, 0, 0, pi/2, -pi/2,0])theta = global_variables.rtde_r.getActualQ()# print("Actual joint angles")# print(theta)U = np.identity(4)for i in reversed(range(6)):U = np.matmul(DH_matrix(theta[i], a[i], d[i], alpha[i]), U)delta = U[2, 0:3]    dd = np.cross(np.matmul(-U[0:3,0:3].T, U[0:3,3]), delta)if(i==5):J66 = np.vstack((dd.reshape([-1,1]), delta.reshape([-1,1])))else:J66 = np.block([np.vstack((dd.reshape([-1,1]), delta.reshape([-1,1]))), J66]) return J66

def DH_matrix(theta, a, d, alpha):    T = np.array([[cos(theta),  -sin(theta)*cos(alpha),  sin(theta)*sin(alpha),   a*cos(theta)],[sin(theta),   cos(theta)*cos(alpha), -cos(theta)*sin(alpha),   a*sin(theta)],[   0.0,                sin(alpha),          cos(alpha),                 d  ],[   0.0,                  0.0,                  0.0,                   1.0  ]])return T

def  Rotation_matrix(Pose):kx, ky, kz = Pose[3], Pose[4], Pose[5]a = sqrt(kx*kx + ky*ky + kz*kz)kx, ky, kz = kx/a, ky/a, kz/aca, sa = cos(a), sin(a)va = 1 - caK2 = np.array([[kx*kx,  kx*ky,  kx*kz],[ky*kx,  ky*ky,  ky*kz],[kz*kx,  kz*ky,  kz*kz]])I = np.identity(3)Kt = np.array([[  0,  -kz,   ky],[ kz,    0,  -kx],[-ky,   kx,    0]])R = ca*I + sa*Kt + va*K2return R