运动学系列 —— 几种雅克比矩阵的区别及RBT中的代码对比

• J0：假设位姿q(1xN)对应的雅克比矩阵(6xN)，N为机器人关节的个数，机器人雅克比矩阵将关节速度与末端执行器空间速度V=J0*qd映射到世界坐标系中，即在末端执行器坐标系中计算雅可比矩阵并将其转换为世界坐标系

• Jn：在末端执行器坐标系中，机械手雅可比矩阵将关节速度映射到末端执行器空间速度V = Jn*qd，这个雅可比矩阵通常被称为几何雅可比矩阵。

• Jdot：雅可比矩阵(q, qd)是雅可比矩阵(在世界坐标系中)和关节速率的导数(6×1)的乘积。用于操作空间控制 Xdd = J(q)qdd + Jdot(q)qd

  Robotics Toolbox：  

• 9.10版本中 Jacobi0：

%SerialLink.JACOB0 Jacobian in world coordinates
%
% J0 = R.jacob0(Q, OPTIONS) is the Jacobian matrix (6xN) for the robot in pose Q (1xN), and N is the number of robot joints.  The manipulatorJacobian matrix maps joint velocity to end-effector spatial velocity V =J0*QD expressed in the world-coordinate frame.
% j?= R。雅可比矩阵(Q, OPTIONS)为位姿Q (1xN)中机器人的雅可比矩阵(6xN)， N为机器人关节的个数。机械手雅可比矩阵将关节速度与末端执行器空间速度V =J0*QD映射到世界坐标系中。
%
% Options::
% 'rpy'     Compute analytical Jacobian with rotation rate in terms of 
%           roll-pitch-yaw angles
% 'eul'     Compute analytical Jacobian with rotation rates in terms of 
%           Euler angles
% 'trans'   Return translational submatrix of Jacobian
% 'rot'     Return rotational submatrix of Jacobian 
%
% Note::
% 在末端执行器坐标系中计算雅可比矩阵并将其转换为世界坐标系
% 返回的缺省雅可比矩阵通常称为几何雅可比矩阵，而不是解析雅可比矩阵。

function J0 = jacob0(robot, q, varargin)

    opt.rpy = false;
    opt.eul = false;
    opt.trans = false;
    opt.rot = false;
    
    opt = tb_optparse(opt, varargin);
    
	%
	%   dX_tn = Jn dq
	%
	Jn = jacobn(robot, q);	% Jacobian from joint to wrist space

	%
	%  convert to Jacobian in base coordinates
	%
	Tn = fkine(robot, q);	% end-effector transformation
	R = t2r(Tn);
	J0 = [R zeros(3,3); zeros(3,3) R] * Jn;

    if opt.rpy
        rpy = tr2rpy( fkine(robot, q) );
        B = rpy2jac(rpy);
        if rcond(B) < eps
            error('Representational singularity');
        end
        J0 = blkdiag( eye(3,3), inv(B) ) * J0;
    elseif opt.eul
        eul = tr2eul( fkine(robot, q) );
        B = eul2jac(eul);
        if rcond(B) < eps
            error('Representational singularity');
        end
        J0 = blkdiag( eye(3,3), inv(B) ) * J0;
    end
    
    if opt.trans
        J0 = J0(1:3,:);
    elseif opt.rot
        J0 = J0(4:6,:);
    end

  jacobin：  

%SerialLink.JACOBN Jacobian in end-effector frame
%
% JN = R.jacobn(Q, options) is the Jacobian matrix (6xN) for the robot in pose Q, and N is the number of robot joints. The manipulator Jacobian matrix maps joint velocity to end-effector spatial velocity V = JN*QD in the end-effector frame.
%雅可比矩阵(Q, options)是位姿Q下机器人的雅可比矩阵(6xN)， N是机器人关节的个数。在末端执行器坐标系中，机械手雅可比矩阵将关节速度映射到末端执行器空间速度V = JN*QD。
%
% Options::
% 'trans'   Return translational submatrix of Jacobian
% 'rot'     Return rotational submatrix of Jacobian 
%
% Notes::
% 这个雅可比矩阵通常被称为几何雅可比矩阵。


function J = jacobn(robot, q, varargin)

    opt.trans = false;
    opt.rot = false;
    
    opt = tb_optparse(opt, varargin);
    
    n = robot.n;
    L = robot.links;        % get the links
    
    if isa(q, 'sym')
        J(6, robot.n) = sym();
    else
        J = zeros(6, robot.n);
    end
    U = robot.tool;
    for j=n:-1:1
        if robot.mdh == 0
            % standard DH convention
            U = L(j).A(q(j)) * U;
        end
        if L(j).RP == 'R'
            % revolute axis
            d = [   -U(1,1)*U(2,4)+U(2,1)*U(1,4)
                -U(1,2)*U(2,4)+U(2,2)*U(1,4)
                -U(1,3)*U(2,4)+U(2,3)*U(1,4)];
            delta = U(3,1:3)';  % nz oz az
        else
            % prismatic axis
            d = U(3,1:3)';      % nz oz az
            delta = zeros(3,1); %  0  0  0
        end
        J(:,j) = [d; delta];

        if robot.mdh ~= 0
            % modified DH convention
            U = L(j).A(q(j)) * U;
        end
    end
    
    if opt.trans
        J = J(1:3,:);
    elseif opt.rot
        J = J(4:6,:);
    end
    
    if isa(J, 'sym')
        J = simplify(J);
    end

  jacobi_dot  

%SerialLink.jacob_dot Derivative of Jacobian
%
% JDQ = R.jacob_dot(Q, QD) is the product (6x1) of the derivative of the Jacobian (in the world frame) and the joint rates.
% JDQ = R。雅可比矩阵(Q, QD)是雅可比矩阵(在世界坐标系中)和关节速率的导数(6x1)的乘积。
% Notes::
% - Useful for operational space control用于操作空间控制 XDD = J(Q)QDD + JDOT(Q)QD
% - Written as per the reference and not very efficient.

function Jdot = jacob_dot(robot, q, qd)

	n = robot.n;
    links = robot.links;

    % Using the notation of Angeles:
    %   [Q,a] ~ [R,t] the per link transformation
    %   P ~ R   the cumulative rotation t2r(Tj) in world frame
    %   e       the last column of P, the local frame z axis in world coordinates
    %   w       angular velocity in base frame
    %   ed      deriv of e
    %   r       is distance from final frame
    %   rd      deriv of r
    %   ud      ??

    for i=1:n
        T = links(i).A(q(i));
        Q{i} = t2r(T);
        a{i} = transl(T);
    end

    P{1} = Q{1};
    e{1} = [0 0 1]';
    for i=2:n
        P{i} = P{i-1}*Q{i};
        e{i} = P{i}(:,3);
    end

    % step 1
    w{1} = qd(1)*e{1};
    for i=1:(n-1)
        w{i+1} = qd(i+1)*[0 0 1]' + Q{i}'*w{i};
    end

    % step 2
    ed{1} = [0 0 0]';
    for i=2:n
        ed{i} = cross(w{i}, e{i});
    end

    % step 3
    rd{n} = cross( w{n}, a{n});
    for i=(n-1):-1:1
        rd{i} = cross(w{i}, a{i}) + Q{i}*rd{i+1};
    end

    r{n} = a{n};
    for i=(n-1):-1:1
        r{i} = a{i} + Q{i}*r{i+1};
    end

    ud{1} = cross(e{1}, rd{1});
    for i=2:n
        ud{i} = cross(ed{i}, r{i}) + cross(e{i}, rd{i});
    end

    % step 4
    %  swap ud and ed
    v{n} = qd(n)*[ud{n}; ed{n}];
    for i=(n-1):-1:1
        Ui = blkdiag(Q{i}, Q{i});
        v{i} = qd(i)*[ud{i}; ed{i}] + Ui*v{i+1};
    end

    Jdot = v{1};

• 10.x 版本中

jacobi0  

%SerialLink.JACOB0 Jacobian in world coordinates
%
% J0 = R.jacob0(Q, OPTIONS) is the Jacobian matrix (6xN) for the robot in
% pose Q (1xN), and N is the number of robot joints.  The manipulator
% Jacobian matrix maps joint velocity to end-effector spatial velocity V =
% J0*QD expressed in the world-coordinate frame.
%
% Options::
% 'rpy'     Compute analytical Jacobian with rotation rate in terms of 
%           XYZ roll-pitch-yaw angles
% 'eul'     Compute analytical Jacobian with rotation rates in terms of 
%           Euler angles
% 'exp'     Compute analytical Jacobian with rotation rates in terms of 
%           exponential coordinates
% 'trans'   Return translational submatrix of Jacobian
% 'rot'     Return rotational submatrix of Jacobian 
%
% Note::
% - End-effector spatial velocity is a vector (6x1): the first 3 elements are translational velocity, the last 3 elements are rotational velocity as angular velocity (default), RPY angle rate or Euler angle rate.
% - This Jacobian accounts for a base and/or tool transform if set.
% - The Jacobian is computed in the end-effector frame and transformed to the world frame.
% - The default Jacobian returned is often referred to as the geometric Jacobian.

function J0 = jacob0(robot, q, varargin)

    opt.trans = false;
    opt.rot = false;
    opt.analytic = {[], 'rpy', 'eul', 'exp'};
    opt.deg = false;
    
    opt = tb_optparse(opt, varargin);
        if opt.deg
    % in degrees mode, scale the columns corresponding to revolute axes
    q = robot.toradians(q);
end
    
	%
	%   dX_tn = Jn dq
	%
	Jn = jacobe(robot, q);	% Jacobian from joint to wrist space

	%
	%  convert to Jacobian in base coordinates
	%
	Tn = fkine(robot, q);	% end-effector transformation
    if isa(Tn, 'SE3')
        R = Tn.R;
    else
        R = t2r(Tn);
    end
	J0 = [R zeros(3,3); zeros(3,3) R] * Jn;

    % convert to analytical Jacobian if required
    if ~isempty(opt.analytic)
        switch opt.analytic
            case 'rpy'
                rpy = tr2rpy(Tn);
                A = rpy2jac(rpy, 'xyz');
                if rcond(A) < eps
                    error('Representational singularity');
                end
            case 'eul'
                eul = tr2eul(Tn);
                A = eul2jac(eul);
                if rcond(A) < eps
                    error('Representational singularity');
                end
            case 'exp'
                [theta,v] = trlog( t2r(Tn) );
                A = eye(3,3) - (1-cos(theta))/theta*skew(v) ...
                    + (theta - sin(theta))/theta*skew(v)^2;
        end
        J0 = blkdiag( eye(3,3), inv(A) ) * J0;
    end
    
    % choose translational or rotational subblocks
    if opt.trans
        J0 = J0(1:3,:);
    elseif opt.rot
        J0 = J0(4:6,:);
    end

  jacobie:

% JE = R.jacobe(Q, options) is the Jacobian matrix (6xN) for the robot in
% pose Q, and N is the number of robot joints. The manipulator Jacobian
% matrix maps joint velocity to end-effector spatial velocity V = JE*QD in
% the end-effector frame.
%
% Options::
% 'trans'   Return translational submatrix of Jacobian
% 'rot'     Return rotational submatrix of Jacobian 
%
% Notes::
% - Was joacobn() is earlier version of the Toolbox.
% - This Jacobian accounts for a tool transform if one is set.
% - This Jacobian is often referred to as the geometric Jacobian.
% - Prior to release 10 this function was named jacobn.

function J = jacobe(robot, q, varargin)

    opt.trans = false;
    opt.rot = false;
    opt.deg = false;
    opt = tb_optparse(opt, varargin);
    if opt.deg
        % in degrees mode, scale the columns corresponding to revolute axes
        q = robot.toradians(q);
    end
    
    n = robot.n;
    L = robot.links;        % get the links
    
    J = zeros(6, robot.n);
    if isa(q, 'sym')
        J = sym(J);
    end
    
    U = robot.tool;
    for j=n:-1:1
        if robot.mdh == 0
            % standard DH convention
            U = L(j).A(q(j)) * U;
        end
        
        UT = U.T;

        if L(j).isrevolute
            % revolute axis
            d = [   -UT(1,1)*UT(2,4)+UT(2,1)*UT(1,4)
                -UT(1,2)*UT(2,4)+UT(2,2)*UT(1,4)
                -UT(1,3)*UT(2,4)+UT(2,3)*UT(1,4)];
            delta = UT(3,1:3)';  % nz oz az
        else
            % prismatic axis
            d = UT(3,1:3)';      % nz oz az
            delta = zeros(3,1); %  0  0  0
        end
        J(:,j) = [d; delta];

        if robot.mdh ~= 0
            % modified DH convention
            U = L(j).A(q(j)) * U;
        end
    end
    
    if opt.trans
        J = J(1:3,:);
    elseif opt.rot
        J = J(4:6,:);
    end
    
    if isa(J, 'sym')
        J = simplify(J);
    end

  jacobi_dot  

%SerialLink.jacob_dot Derivative of Jacobian

% JDQ = R.jacob_dot(Q, QD) is the product (6x1) of the derivative of the
% Jacobian (in the world frame) and the joint rates.

function Jdot = jacob_dot(robot, q, qd)

	n = robot.n;
    links = robot.links;

    % Using the notation of Angeles:
    %   [Q,a] ~ [R,t] the per link transformation
    %   P ~ R   the cumulative rotation t2r(Tj) in world frame
    %   e       the last column of P, the local frame z axis in world coordinates
    %   w       angular velocity in base frame
    %   ed      deriv of e
    %   r       is distance from final frame
    %   rd      deriv of r
    %   ud      ??

    for i=1:n
        T = links(i).A(q(i));
        Q{i} = t2r(T);
        a{i} = transl(T)';
    end

    P{1} = Q{1};
    e{1} = [0 0 1]';
    for i=2:n
        P{i} = P{i-1}*Q{i};
        e{i} = P{i}(:,3);
    end

    % step 1
    w{1} = qd(1)*e{1};
    for i=1:(n-1)
        w{i+1} = qd(i+1)*[0 0 1]' + Q{i}'*w{i};
    end

    % step 2
    ed{1} = [0 0 0]';
    for i=2:n
        ed{i} = cross(w{i}, e{i});
    end

    % step 3
    rd{n} = cross( w{n}, a{n});
    for i=(n-1):-1:1
        rd{i} = cross(w{i}, a{i}) + Q{i}*rd{i+1};
    end

    r{n} = a{n};
    for i=(n-1):-1:1
        r{i} = a{i} + Q{i}*r{i+1};
    end

    ud{1} = cross(e{1}, rd{1});
    for i=2:n
        ud{i} = cross(ed{i}, r{i}) + cross(e{i}, rd{i});
    end

    % step 4
    %  swap ud and ed
    v{n} = qd(n)*[ud{n}; ed{n}];
    for i=(n-1):-1:1
        Ui = blkdiag(Q{i}, Q{i});
        v{i} = qd(i)*[ud{i}; ed{i}] + Ui*v{i+1};
    end

    Jdot = v{1};