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

ラ@ @ブスクリプトを開く

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

ロボットモデルの設定

この例では7自由度のロボットマニピュレーターである库卡LBR iiwaのモデルを使用します。importrobotは、统一机器人描述格式(URDF)ファイルに保存されている記述からrigidBodyTreeモデルを生成します。

LBR = importrobot(“iiwa14.urdf”）;% 14公斤有效载荷版本lbr。DataFormat =“行”;爪=“iiwa_link_ee_kuka”;

カップの寸法を定義します。

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

カップの中心を表すロボットモデルに固定されたボディを追加します。

body = rigidBody(“cupFrame”）;setFixedTransform(身体。join, trvec2tform(cupPosition)) addBody(lbr, body, lbr. basename);

計画問題の定義

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

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

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

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

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

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

gik =广义逆运动学(“RigidBodyTree”lbr,…“ConstraintInputs”, {笛卡儿的，“位置”，“目标”，“定位”，“联合”}）

gik = generalizedInverseKinematics with properties: NumConstraints: 5 ConstraintInputs: {1x5 cell} RigidBodyTree: [1x1 RigidBodyTree] SolverAlgorithm: 'BFGSGradientProjection' SolverParameters: [1x1 struct]

拘束オブジェクトの作成

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

グリッパーが少なくともテーブル上5厘米(負のz方向)となるよう要求する,直交座標の範囲拘束を作成します。その他すべての値は正または负として指定します。

heightAboveTable = constraintCartesianBounds(夹具);heightAboveTable。界限= [-inf, inf;…负无穷到正无穷;…0.05,正]

heightAboveTable = constraintCartesianBounds with properties: enffector: 'iiwa_link_ee_kuka' ReferenceBody: " TargetTransform: [4x4 double] Bounds: [3x2 double] Weights: [1 1 1 1]

グリッパ，に対するカップの位置の拘束を，許容誤差5mmで作成します。

distanceFromCup up = constraintPositionTarget(“cupFrame”）;distanceFromCup。ReferenceBody =夹持器;distanceFromCup。位置公差= 0.005

distanceFromCup = constraintPositionTarget with properties: EndEffector: 'cupFrame' ReferenceBody: 'iiwa_link_ee_kuka' TargetPosition: [0 00 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: enffector: 'iiwa_link_ee' ReferenceBody: " TargetPoint: [0 0 100] AngularTolerance: 0 Weights: 1

ジョ@ @ント位置の範囲の拘束を作成します。ジョ界限プロパティを設定します。

limitJointChange = constraintJointBounds(lbr)

limitJointChange = constraintJointBounds with properties: Bounds: [7x2 double] Weights: [1 1 1 1 1 1 1 1 1]

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

fixOrientation = constraintOrientationTarget(夹具);fixOrientation。方位公差=度数(1)

fixOrientation = constraintOrientationTarget with properties: EndEffector: 'iiwa_link_ee_kuka' ReferenceBody: " TargetOrientation: [1 0 0 0] OrientationTolerance: 0.0175 Weights: 1

カップを指すコンフィギュレ，ションを求める

このコンフィギュレーションは,グリッパーの位置を適切に揃えて最終的な接近が行えるように,グリッパーをカップから少し離して配置します。

intermediateDistance = 0.3;

拘束オブジェクトには，競合する拘束をソルバ，がどのように処理するかを決定する权重プロパティがあります。拘束の重みを0に設定すると，拘束が無効になります。このコンフィギュレションでは，ジョントの位置範囲と向きの拘束を無効にします。

limitJointChange。权重= zeros(size(limitJointChange.Weights)); fixOrientation.Weights = 0;

カップのタ，ゲット位置をグリッパ，の座標系内で設定します。カップはグリッパのz軸上の指定された距離になければなりません。

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

gikソルバ，を使用して，入力拘束を満たすロボットコンフィギュレ，ションを求めます。すべての入力拘束を指定しなければなりません。そのコンフィギュレションを2番目のウェポントとして設定します。

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

グリッパ，をカップまで直線に沿って動かすコンフィギュレ，ションを求める

ジョ@ @ントの位置範囲と向きの拘束を再度有効にします。

limitJointChange。权重= ones(size(limitJointChange.Weights)); fixOrientation.Weights = 1;

カップと揃えるという拘束は向きの拘束により冗長になるため，無効にします。

alignWithCup。权重= 0;

向きの拘束を設定し，前述のコンフィギュレ，ション(: qWaypoints (2))に基づいて向きを保持します。グリッパ，からロボットモデルのベ，スへの変換を取得します。同次変換を四元数に変換します。

fixOrientation。TargetOrientation =…tform2quat (getTransform (lbr qWaypoints(2:),爪));

各ウェ@ @ポ@ @ントでのカップとグリッパ@ @の距離を定義します。

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

各ウェ▪▪ポ▪▪ントの間におけるジョ▪▪ント位置の最大許容変化を定義します。

maxJointChange = deg2rad(10);

残りの各ウェ▪▪ポ▪▪ントに▪。

为k = 3:numWaypoints更新目标位置。distanceFromCupValues . targetposition (3) = distanceFromCupValues(k-1);%限制关节位置，使其接近先前的值。limitJointChange。界限= [qWaypoints(k-1,:)' - maxJointChange,…qWaypoints(k-1，:)' + maxJointChange];求解一个配置并将其添加到waypoints数组中。[qWaypoints(k，:)，solutionInfo] = gik(qWaypoints(k-1，:))，…heightAboveTable,…distanceFromCup alignWithCup,…fixOrientation limitJointChange);结束

生成された軌跡の可視化

ウェ▪▪ポ▪▪ントの間を内挿して滑らかな軌跡を生成します。pchip。

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

内挿されたそれぞれのコンフィギュレションにいてグリッパの位置を計算します。

gripperPosition = 0 (numFrames,3);为k = 1:numFrames grip (k，:) = tform2trvec(getTransform(lbr,qInterp(k，:))，…爪));结束

初期コンフィギュレ，ションのロボットを，テ，ブルとカップと共に表示します。

图;显示(lbr qWaypoints (1:)“PreservePlot”、假);持有在exampleHelperPlotCupAndTable(cupHeight, cupRadius, cupPosition);p = plot3(gripperPosition(1,1)， gripperPosition(1,2)， gripperPosition(1,3));

マニピュレ，タ，をアニメ，ション化し，グリッパ，の位置をプロットします。

持有在为k = 1:size(qInterp,1) show(lbr, qInterp(k，:)“PreservePlot”、假);p.XData(k) = grip (k,1);p.YData(k) = grip (k,2);p.ZData(k) = grip (k,3);等待(r);结束持有从

生成されたコンフィギュレーションを後で使用するために垫ファイルに保存する場合は,以下を実行します。

> > (lbr_trajectory攒钱。mat'， 'tWaypoints'， 'qWaypoints');