Robotics System Toolbox学习笔记（六）：gik函数例子：Solve Inverse Kinematics for a Four-Bar Linkage（平面闭合链机构）

本示例说明了如何解决四连杆机构（简单的平面闭合链机构）的逆运动学问题。 Robotics System Toolbox™不直接支持闭环机制。但是，可以使用运动学约束来近似闭环关节。本示例说明如何为四连杆机构设置刚体树，指定运动学约束并求解所需的末端执行器位置。 准备知识： Robotics System Toolbox学习笔记（五）：generalizedInverseKinematics建立多约束逆运动学求解器 Robotics System Toolbox学习笔记（四）：inverseKinematics及逆运动学约束ConstraintInputs  

% 初始化四连杆机构刚体树模型
robot = rigidBodyTree('Dataformat','column','MaxNumBodies',7);

% 定义主体名称，父名称，关节名称，关节类型和固定变换
% 固定的变换定义了四连杆机构的几何形状。联动装置在xz平面中旋转。
% 在“ b4”实体的y轴上使用-0.1的偏移量来隔离“ b3”和“ b4”的重叠关节的运动。
bodyNames = {'b1','b2','b3','b4','b5','b6'};
parentNames = {'base','b1','b2','base','b4','b5'};
jointNames = {'j1','j2','j3','j4','j5','j6'};
jointTypes = {'revolute','revolute','fixed','revolute','revolute','fixed'};
fixedTforms = {eye(4), ...
                trvec2tform([0 0 0.5]), ...
                trvec2tform([0.8 0 0]), ...
                trvec2tform([0.0 -0.1 0]), ...
                trvec2tform([0.8 0 0]), ...
                trvec2tform([0 0 0.5])};

% 使用for循环组装四杆连杆：
%   创建刚体并指定关节类型
%   为任何非固定关节指定JointAxis属性   
%   指定固定的转换。     
%   将实体添加到刚体树中。
for k = 1:6

    b = rigidBody(bodyNames{k});
    b.Joint = rigidBodyJoint(jointNames{k},jointTypes{k});
    
    if ~strcmp(jointTypes{k},'fixed')
        b.Joint.JointAxis = [0 1 0];
    end
    
    b.Joint.setFixedTransform(fixedTforms{k});
    
    addBody(robot,b,parentNames{k});
end
% 添加一个最终主体，作为四连杆机构的末端执行器（手柄）
bn = 'handle';
b = rigidBody(bn);
% 创建关节的固定变换属性
% setFixedTransform(jointObj,tform)直接使用提供的tform齐次变换矩阵设置rigidBodyJoint对象jointObj的JointToParentTransform属性
setFixedTransform(b.Joint,trvec2tform([0 -0.15 0]));
addBody(robot,b,'b6');

% 为GeneralizedInverseKinematics对象指定运动学约束
gik = generalizedInverseKinematics('RigidBodyTree',robot);
gik.ConstraintInputs = {'position',...  % Position constraint for closed-loop mechanism
                        'position',...  % Position constraint for end-effector 
                        'joint'};       % Joint limits
gik.SolverParameters.AllowRandomRestart = false;

% 位置限制1：'b3'主体框架和'b6'主体框架的原点应始终重叠。这使手柄与近似的闭环机构保持一致。
positionTarget1 = constraintPositionTarget('b6','ReferenceBody','b3');
% 将-0.1偏移量用作y坐标。
positionTarget1.TargetPosition = [0 -0.1 0];
positionTarget1.Weights = 50;
positionTarget1.PositionTolerance = 1e-6;

% 关节极限范围：满足刚体树模型中的关节极限。
jointLimBounds = constraintJointBounds(gik.RigidBodyTree);
jointLimBounds.Weights = ones(1,size(gik.RigidBodyTree.homeConfiguration,1))*10;

% 位置限制2：末端执行器应定位到所需位置。
desiredEEPosition = [0.9 -0.1 0.9]'; % Position is relative to base.
positionTarget2 = constraintPositionTarget('handle');
positionTarget2.TargetPosition = desiredEEPosition; 
positionTarget2.PositionTolerance = 1e-6;
positionTarget2.Weights = 1;

% 使用gik对象计算运动学解。以适当的顺序指定初始猜测和不同的运动学约束。
iniGuess = homeConfiguration(robot);
[q, solutionInfo] = gik(iniGuess,positionTarget1,positionTarget2,jointLimBounds);
% 检查solutionInfo中的结果。显示与home configuration相比的运动学解决方案。绘图显示在xz平面中。
loopClosingViolation = solutionInfo.ConstraintViolations(1).Violation;
jointBndViolation = solutionInfo.ConstraintViolations(2).Violation;
eePositionViolation = solutionInfo.ConstraintViolations(3).Violation;

subplot(1,2,1)
show(robot,homeConfiguration(robot));
title('Home Configuration')
view([0 -1 0]);
subplot(1,2,2)
show(robot,q);
title('GIK Solution')
view([0 -1 0]);