50 lines
2.2 KiB
Mathematica
50 lines
2.2 KiB
Mathematica
|
function [jacoInfo] = calculateJacobian(robotRigidInfo,...
|
|||
|
|
rotInfo,...
|
|||
|
|
theta)
|
|||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|||
|
|
% 计算机器人各个轴臂的雅可比矩阵，
|
|||
|
|
% dynamicBasicInfo: 机械臂的基本信息
|
|||
|
|
% rotInfo: 各个轴臂的姿态信息
|
|||
|
|
% theta: 转角
|
|||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|||
|
|
temp = struct;
|
|||
|
|
temp.jaco = zeros(6,9);
|
|||
|
|
jacoInfo = struct;
|
|||
|
|
jacoInfo.link = ([temp temp temp temp temp temp temp temp temp]);
|
|||
|
|
jacoInfo.motor = ([temp temp temp temp temp temp temp temp temp]);
|
|||
|
|
for i=1:9
|
|||
|
|
jacoInfo.link(i).jaco = zeros(6,9);
|
|||
|
|
jacoInfo.motor(i).jaco = zeros(6,9);
|
|||
|
|
comToRotCenter = -rotInfo.link(i).rot*robotRigidInfo.link(i).R1;
|
|||
|
|
for j=i:-1:1
|
|||
|
|
if robotRigidInfo.link(j).jointAxis>3
|
|||
|
|
rotAxis=zeros(3,1);
|
|||
|
|
rotAxis(robotRigidInfo.link(j).jointAxis-3) = 1;
|
|||
|
|
% movement
|
|||
|
|
jacoInfo.link(i).jaco(1:3,j) = -vector2so3(comToRotCenter)*rotInfo.link(j).rot*rotAxis;
|
|||
|
|
if j>1
|
|||
|
|
comToRotCenter = comToRotCenter + ...
|
|||
|
|
rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ...
|
|||
|
|
- rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1;
|
|||
|
|
end
|
|||
|
|
% rotation
|
|||
|
|
jacoInfo.link(i).jaco(4:6,j) = rotInfo.link(i).rotTran*rotInfo.link(j).rot*rotAxis;
|
|||
|
|
else
|
|||
|
|
tranAxis=zeros(3,1);
|
|||
|
|
tranAxis(robotRigidInfo.link(j).jointAxis) = 1;
|
|||
|
|
% movement
|
|||
|
|
jacoInfo.link(i).jaco(1:3,j) = rotInfo.link(j).rot*tranAxis;
|
|||
|
|
if j>1
|
|||
|
|
comToRotCenter = comToRotCenter + ...
|
|||
|
|
rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ...
|
|||
|
|
- rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1 + ...
|
|||
|
|
rotInfo.link(j).rot*tranAxis*theta(j);
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
motorRotAxis = zeros(3,1);
|
|||
|
|
motorRotAxis(robotRigidInfo.motor(i).jointAxis-3) = 1;
|
|||
|
|
jacoInfo.motor(i).jaco = jacoInfo.link(i).jaco;
|
|||
|
|
jacoInfo.motor(i).jaco(4:6,i) = robotRigidInfo.motor(i).gearRatio*motorRotAxis;
|
|||
|
|
end
|