複数の運動学的拘束をもつリーチ軌跡の計画

ライブスクリプトを開く

この例では、汎用逆運動学を使用して、ロボットマニピュレーターのジョイント空間での軌跡を計画する方法を説明します。複数の拘束を組み合わせて、グリッパーをテーブル上のカップに誘導する軌跡を生成します。これらの拘束により、グリッパーの姿勢を事前に決定しなくとも、グリッパーは確実にカップへと真っすぐ近づき、またグリッパーはテーブルから安全な距離を確保します。

ロボットモデルの設定

この例では、7 自由度のロボットマニピュレーターである KUKA LBR iiwa のモデルを使用します。importrobotは、Unified Robot Description Format (URDF) ファイルに保存されている記述から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);

計画問題の定義

この例の目標は、以下の条件を満たす一連のロボットコンフィギュレーションを生成することです。

ホームコンフィギュレーションで開始する
ロボットコンフィギュレーションに突然の変化はない
グリッパーを少なくとも "テーブル" (z = 0) 上 5 cm の高さに維持する
グリッパーはカップに近づくにつれて、カップと位置を揃えなければならない
カップの中心から 5 cm の位置でグリッパーを終了する

この例では、拘束オブジェクトを使用して、これらの条件を満たすロボットコンフィギュレーションを生成します。生成された軌跡は、5 つのコンフィギュレーションウェイポイントで構成されます。最初のウェイポイント q0 は、ホームコンフィギュレーションとして設定されます。repmat を使用して、残りのコンフィギュレーションを qWaypoints に事前に割り当てます。

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

次の拘束入力を受け入れるgeneralizedInverseKinematicsソルバーを作成します。

直交座標の範囲 - グリッパーの高さを制限する
位置ターゲット - グリッパーに対するカップの位置を指定する
照準拘束 - グリッパーをカップの軸と揃える
方向ターゲット - カップへの接近中に、グリッパーの方向を固定して維持する
ジョイント位置の範囲 - ウェイポイントの間におけるジョイント位置の変化を制限する

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

gik = 
  generalizedInverseKinematics with properties:

      NumConstraints: 5
    ConstraintInputs: {'cartesian'  'position'  'aiming'  'orientation'  'joint'}
       RigidBodyTree: [1x1 rigidBodyTree]
     SolverAlgorithm: 'BFGSGradientProjection'
    SolverParameters: [1x1 struct]

拘束オブジェクトの作成

入力としてソルバーに渡される拘束オブジェクトを作成します。これらのオブジェクトには、各拘束に必要なパラメーターが含まれます。必要に応じて、ソルバー呼び出しの合い間にこれらのパラメーターを変更します。

グリッパーが少なくともテーブル上 5 cm (負の 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 mm で作成します。

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]

許容誤差 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 ソルバーを使用して、入力拘束を満たすロボットコンフィギュレーションを求めます。すべての入力拘束を指定しなければなりません。そのコンフィギュレーションを 2 番目のウェイポイントとして設定します。

[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');