29 lines
1.3 KiB
Matlab
29 lines
1.3 KiB
Matlab
function [rotInfo] = calculateArmRot(dynamicBasicInfo,theta)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% 计算机器人各个轴臂的姿态，四元素/旋转矩阵...
|
|
% dynamicBasicInfo: PSA机械臂的基本信息
|
|
% setupInfo: SUA(setup arm)状态，各个轴的角度等 []
|
|
% state:系统动力学的状态变量
|
|
% 假设：关节变形和柔性体变形都是小量，忽略其对姿态的影响
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
rotInfo = struct;
|
|
temp = struct;
|
|
temp.rot = eye(3);
|
|
temp.rotTran = eye(3);
|
|
rotInfo.link = ([temp temp temp temp temp temp temp temp temp]);
|
|
rotInfo.motor = ([temp temp temp temp temp temp temp temp temp]);
|
|
rotInfo.link(1).rot = rot(dynamicBasicInfo.link(1).jointAxis,theta(1));
|
|
rotInfo.link(1).rotTran = rotInfo.link(1).rot';
|
|
|
|
rotInfo.motor(1).rot = rotInfo.link(1).rot;
|
|
rotInfo.motor(1).rotTran = rotInfo.motor(1).rot';
|
|
for i=2:9
|
|
if dynamicBasicInfo.link(i).jointAxis>3
|
|
rotInfo.link(i).rot = rotInfo.link(i-1).rot*rot(dynamicBasicInfo.link(i).jointAxis-3,theta(i));
|
|
else
|
|
rotInfo.link(i).rot = rotInfo.link(i-1).rot;
|
|
end
|
|
rotInfo.link(i).rotTran = rotInfo.link(i).rot';
|
|
rotInfo.motor(i).rot = rotInfo.link(i).rot;
|
|
rotInfo.motor(i).rotTran = rotInfo.motor(i).rot';
|
|
end |