- 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};