设计具有多个运动学约束的到达轨迹

如何使用广义逆运动学来规划一个机械臂的关节空间轨迹？

结合多个约束条件，生成一个轨迹，将夹持器导向放在桌子上的杯子。这些约束保证了夹持器以直线接近杯子，并且夹持器保持在与桌子的安全距离，而不需要事先确定夹持器的姿态。

设置机器人模型

使用KUKA LBR iiwa(7自由度机器人机械手)的模型。importrobot从存储在URDF (Unified Robot description Format)文件中的描述中生成rigidBodyTree模型。

lbr = importrobot('iiwa14.urdf'); % 14 kg payload version
lbr.DataFormat = 'row';
gripper = 'iiwa_link_ee_kuka';

定义杯子的尺寸：

cupHeight = 0.2;
cupRadius = 0.05;
cupPosition = [-0.5, 0.5, cupHeight/2];

在机器人模型中添加一个固定的物体来代表杯子的中心

body = rigidBody('cupFrame');
setFixedTransform(body.Joint, trvec2tform(cupPosition))
addBody(lbr, body, lbr.BaseName);

描述规划问题：

目标是生成一系列满足以下条件的机器人构型:

（1）在主配置中启动

（2）机器人构型无突变

（3）保持夹持器距“桌子”至少5厘米(z = 0)

（4）当杯子靠近时，手爪应该与杯子对齐

（5）用夹持器从杯子中心5厘米处完成

用约束对象来生成满足这些条件的机器人配置。

生成的轨迹由5个构型路标点组成。

第一个路径点q0被设置为home配置。使用repmat在qwaypoint中预分配其余配置。

numWaypoints = 5;
q0 = homeConfiguration(lbr);
qWaypoints = repmat(q0, numWaypoints, 1);

创建一个接受以下约束输入的generalizedinverse运动学求解器:

（1）笛卡尔界限-限制抓手的高度

（2）位置目标-指定杯子相对于夹持器的位置

（3）目标约束-将夹持器对准杯轴

（4）定位目标-在接近杯子时保持抓手的固定方向

（5）关节位置边界-限制路点之间关节位置的变化

gik = generalizedInverseKinematics('RigidBodyTree', lbr, ...
    'ConstraintInputs', {'cartesian','position','aiming','orientation','joint'})

gik = 
  generalizedInverseKinematics with properties:

      NumConstraints: 5
    ConstraintInputs: {1x5 cell}
       RigidBodyTree: [1x1 rigidBodyTree]
     SolverAlgorithm: 'BFGSGradientProjection'
    SolverParameters: [1x1 struct]

创建约束对象

创建作为输入传递给求解器的约束对象。这些对象包含每个约束所需的参数。根据需要在调用求解器之间修改这些参数。

创建一个笛卡尔边界约束，要求夹持器至少高于桌子5厘米(负z方向)。

所有其他值都被赋为inf或-inf

heightAboveTable = constraintCartesianBounds(gripper);
heightAboveTable.Bounds = [-inf, inf; ...
                           -inf, inf; ...
                           0.05, inf]

heightAboveTable = 
  constraintCartesianBounds with properties:

        EndEffector: 'iiwa_link_ee_kuka'
      ReferenceBody: ''
    TargetTransform: [4x4 double]
             Bounds: [3x2 double]
            Weights: [1 1 1]

在杯子相对于夹持器的位置上创建一个约束，公差为5毫米。

distanceFromCup = constraintPositionTarget('cupFrame');
distanceFromCup.ReferenceBody = gripper;
distanceFromCup.PositionTolerance = 0.005

distanceFromCup = 
  constraintPositionTarget with properties:

          EndEffector: 'cupFrame'
        ReferenceBody: 'iiwa_link_ee_kuka'
       TargetPosition: [0 0 0]
    PositionTolerance: 0.0050
              Weights: 1

通过将目标放置在机器人上方，创建一个瞄准约束，要求iiwa_link_ee框架的z轴近似垂直。iiwa_link_ee框架是定向的，这样的约束使夹持器与杯轴对齐。

alignWithCup = constraintAiming('iiwa_link_ee');
alignWithCup.TargetPoint = [0, 0, 100]

alignWithCup = 
  constraintAiming with properties:

         EndEffector: 'iiwa_link_ee'
       ReferenceBody: ''
         TargetPoint: [0 0 100]
    AngularTolerance: 0
             Weights: 1

创建关节位置边界约束。根据前面的配置设置此约束的Bounds属性，以限制关节位置的变化。

limitJointChange = constraintJointBounds(lbr)

limitJointChange = 
  constraintJointBounds with properties:

     Bounds: [7x2 double]
    Weights: [1 1 1 1 1 1 1]

为夹持器创建一个方位约束，公差为一度。这个约束要求夹持器的方向与TargetOrientation属性指定的值相匹配。使用这个约束来固定夹具的方向在最后接近杯子。

fixOrientation = constraintOrientationTarget(gripper);
fixOrientation.OrientationTolerance = deg2rad(1)

fixOrientation = 
  constraintOrientationTarget with properties:

             EndEffector: 'iiwa_link_ee_kuka'
           ReferenceBody: ''
       TargetOrientation: [1 0 0 0]
    OrientationTolerance: 0.0175
                 Weights: 1

找到一个指向杯子的配置

这种配置应该将夹持器放置在与杯子的一段距离，以便最终接近可以与夹持器正确对齐。

intermediateDistance = 0.3;

约束对象有一个Weights属性，它决定了求解器如何处理冲突的约束。将约束的权重设置为0将禁用约束。对于此配置，禁用关节位置边界和方向约束。

limitJointChange.Weights = zeros(size(limitJointChange.Weights));
fixOrientation.Weights = 0;

将杯子的目标位置放在夹持架上。杯子应在指定的距离上位于夹持器的z轴上。

distanceFromCup.TargetPosition = [0,0,intermediateDistance];

使用gik求解器求解满足输入约束的机器人构型。必须指定所有的输入约束。将该配置设置为第二个路径点。

[qWaypoints(2,:),solutionInfo] = gik(q0, heightAboveTable, ...
                       distanceFromCup, alignWithCup, fixOrientation, ...
                       limitJointChange);

找到沿直线移动抓手到杯子的构型

重新启用关节位置约束和方向约束

limitJointChange.Weights = ones(size(limitJointChange.Weights));
fixOrientation.Weights = 1;

不需要与杯对齐约束，因为方向约束使其冗余。

alignWithCup.Weights = 0;

定义方向约束以保持基于前面配置的方向(qWaypoints(2，:)))。

得到了机器人模型从抓手到底座的转换。将齐次变换转换为四元数。

fixOrientation.TargetOrientation = ...
    tform2quat(getTransform(lbr,qWaypoints(2,:),gripper));

定义每个路点的杯子和夹持器之间的距离

finalDistanceFromCup = 0.05;
distanceFromCupValues = linspace(intermediateDistance, finalDistanceFromCup, numWaypoints-1);

定义每个路点之间关节位置的最大允许变化

maxJointChange = deg2rad(10);

为每个剩余的路径点调用求解器。

for k = 3:numWaypoints
    % Update the target position.
    distanceFromCup.TargetPosition(3) = distanceFromCupValues(k-1);
    % Restrict the joint positions to lie close to their previous values.
    limitJointChange.Bounds = [qWaypoints(k-1,:)' - maxJointChange, ...
                               qWaypoints(k-1,:)' + maxJointChange];
    % Solve for a configuration and add it to the waypoints array.
    [qWaypoints(k,:),solutionInfo] = gik(qWaypoints(k-1,:), ...
                                         heightAboveTable, ...
                                         distanceFromCup, alignWithCup, ...
                                         fixOrientation, limitJointChange);
end

可视化生成的轨迹

在路径点之间插值生成一个平滑的轨迹。使用pchip避免过冲，可能会违反机器人的关节极限。

framerate = 15;
r = rateControl(framerate);
tFinal = 10;
tWaypoints = [0,linspace(tFinal/2,tFinal,size(qWaypoints,1)-1)];
numFrames = tFinal*framerate;
qInterp = pchip(tWaypoints,qWaypoints',linspace(0,tFinal,numFrames))';

计算每个内插配置的夹持器位置

gripperPosition = zeros(numFrames,3);
for k = 1:numFrames
    gripperPosition(k,:) = tform2trvec(getTransform(lbr,qInterp(k,:), ...
                                                    gripper));
end

展示机器人的初始配置以及桌子和杯子

figure;
show(lbr, qWaypoints(1,:), 'PreservePlot', false);
hold on
exampleHelperPlotCupAndTable(cupHeight, cupRadius, cupPosition);
p = plot3(gripperPosition(1,1), gripperPosition(1,2), gripperPosition(1,3));

 动画操作手和绘图抓手的位置

hold on
for k = 1:size(qInterp,1)
    show(lbr, qInterp(k,:), 'PreservePlot', false);
    p.XData(k) = gripperPosition(k,1);
    p.YData(k) = gripperPosition(k,2);
    p.ZData(k) = gripperPosition(k,3);
    waitfor(r);
end
hold off

 如果需要将生成的配置保存到mat文件中以备后续使用，执行以下命令:

save('lbr_trajectory.mat', 'tWaypoints', 'qWaypoints');