机器人雅克比Jacobian矩阵程序

最新推荐文章于 2025-04-27 09:16:06 发布

FL17171314

最新推荐文章于 2025-04-27 09:16:06 发布

阅读量347

点赞数 2

文章标签：机器人矩阵 numpy

本文链接：https://blog.csdn.net/weixin_51367832/article/details/147442363

版权

% 定义机器人的连杆参数
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/a
    ca, sa = cos(a), sin(a)
    va = 1 - ca
    K2 = 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*K2
    return R