最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

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

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

ロボット モデルの設定

この例では、7 自由度のロボット マニピュレーターである KUKA LBR iiwa のモデルを使用します。importrobot は、Unified Robot Description Format (URDF) ファイルに保存されている記述から robotics.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 = robotics.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 = robotics.GeneralizedInverseKinematics('RigidBodyTree', lbr, ...
    'ConstraintInputs', {'cartesian','position','aiming','orientation','joint'})
gik = 
  robotics.GeneralizedInverseKinematics with properties:

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

制約オブジェクトの作成

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

グリッパーが少なくともテーブル上 5 cm (負の z 方向) となるよう要求する、直交座標の範囲制約を作成します。その他すべての値は inf または -inf として指定します。

heightAboveTable = robotics.CartesianBounds(gripper);
heightAboveTable.Bounds = [-inf, inf; ...
                           -inf, inf; ...
                           0.05, inf]
heightAboveTable = 
  CartesianBounds with properties:

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

グリッパーに対するカップの位置の制約を、許容誤差 5 mm で作成します。

distanceFromCup = robotics.PositionTarget('cupFrame');
distanceFromCup.ReferenceBody = gripper;
distanceFromCup.PositionTolerance = 0.005
distanceFromCup = 
  PositionTarget 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 = robotics.AimingConstraint('iiwa_link_ee');
alignWithCup.TargetPoint = [0, 0, 100]
alignWithCup = 
  AimingConstraint with properties:

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

ジョイント位置の範囲の制約を作成します。ジョイント位置の変化を制限する前述のコンフィギュレーションに基づいて、この制約の Bounds プロパティを設定します。

limitJointChange = robotics.JointPositionBounds(lbr)
limitJointChange = 
  JointPositionBounds with properties:

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

許容誤差 1 度で、グリッパーの向きの制約を作成します。この制約は、グリッパーの向きを TargetOrientation プロパティで指定された値に一致させるように要求します。この制約を使用して、カップへの最終的な接近に際してのグリッパーの向きを固定します。

fixOrientation = robotics.OrientationTarget(gripper);
fixOrientation.OrientationTolerance = deg2rad(1)
fixOrientation = 
  OrientationTarget 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 = robotics.Rate(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');