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

ライブスクリプトを开く

こので，逆运动を使用して，ロボットジョイントジョイント空间空间ででののの轨迹轨迹を计画するする方法方法をを说明说明ししますます。。复数复数のののの拘束をて，，，，ます。拘束，グリッパーの姿势姿势に决定なくとも，，グリッパーはは确実确実にににカップカップとと真っすぐ近づき近づき，，，またまたまたグリッパーグリッパーは

ロボットモデルの设定

この例は，，自由自由度ロボットロボットマニピュレーターであるであるであるであるであるであるであるであるであるlimportrobotは，统一机器人说明格式（urdf）ファイルファイル保存れて记述からからrigidBodyTreeモデルを生成ます。

lbr = importrobot('iiwa14.urdf');% 14 kg payload versionlbr.dataformat ='排';gripper ='iiwa_link_ee_kuka';

カップのを定义し。。

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

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

身体=刚性体（'cupFrame'); setFixedTransform(body.Joint, trvec2tform(cupPosition)) addBody(lbr, body, lbr.BaseName);

計画問題の定義

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

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

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

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

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

直交座标 -グリッパー -グリッパーのさををするする
位置 -グリッパー -グリッパーカップの位置をするする
照准 -グリッパー -グリッパーをの轴揃える揃える揃える
方向 -カップへの中に，のを固定してするする
ジョイント位置の範囲 - ウェイポイントの間におけるジョイント位置の変化を制限する

gik =概括性基础化学（“刚性木板”，lbr，...'ConstraintInputs'，，，，{``笛卡尔''，，，，'position'，，，，“瞄准”，，，，'orientation'，，，，'joint'})

gik =具有属性的概括：numconstraints：5个约束inputs：{1x5 cell} arigidbodytree：[1x1 arigidbodytree] solveralgorithm：'bfgsgradientprotoctions'solverParameters'solverParameters：[1x1结构]：[1x1结构]

拘束オブジェクトの作成

としてソルバーれる拘束オブジェクトをしし。これらののは，，各各拘束拘束拘束にに必要パラメーターパラメーターがが含ま含まれれます。。必要にに応じて，ソルバーソルバーの

5厘米（Z Z Zの）とととよう要求，座标范囲拘束作成します。。そのはははinfまたは-infとして指定し。

HeightAbovetable = CondraintCartesianBounds（抓紧）;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]

グリッパーに対するのの拘束を，，许容许容误差误差误差误差误差误差でで作成し

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座标のの轴ほぼ垂直ことを要求照准拘束作成作成し。。。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

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

limitJointChange = constraintJointBounds(lbr)

limitJointChange =带有属性的约束jointbounds：边界：[7x2 double]权重：[1 1 1 1 1 1 1 1 1]

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

fixorientation = CondrientientationTarget（Gripper）;fixorientation.isientationTolerance = deg2rad（1）

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

カップを指すを求める

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

Intermistientistance = 0.3;

拘束オブジェクト，竞合拘束をソルバーどのに处理するかを决定する权重プロパティが。の重みををををににに，拘束が无效にになりなりますます。。。。このコンフィギュレーションでは，，ジョイントジョイントジョイントのの位置位置范囲范囲

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

カップのをの座标系内のののののののののの轴上轴上指定さされたた距离距离にになけれ

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

gikソルバー使用て入力拘束を満たすコンフィギュレーションを求め求めますの入力入力拘束拘束をををを指定指定ししなけれなければなりなりませませんんん2

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

グリッパーをまでに沿っ动かすコンフィギュレーションを求める

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

limitJointChange.Wewights = nones（size（limitjointchange.weights））;fixorientation.jewights = 1;

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

alignWithCup.Weights = 0;

向きのを设定，前述の（Qwaypoints（2，:)）に基づい保持し。グリッパーロボットベースへの変换をを取得します。。同次変换変换変换ををを

fixOrientation.TargetOrientation =...tform2quat（getTransform（lbr，qwaypoints（2，:)，抓手））;

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

FinalDistanceFromCup = 0.05;decortingFromCupValues = linspace（Intermedistance，FinalDistanceFromCup，numwaypoints-1）;

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

maxJointChange = deg2rad(10);

残りの各ウェイポイントについてソルバーを呼び出します。

为了k = 3：numwaypoints% Update the target position.distanceFromCup.TargetPosition(3) = distanceFromCupValues(k-1);％将联合位置限制为接近其先前的值。limitjointchange.bounds = [qwaypoints（k -1，:)' -  maxjointchange，...Qwaypoints（k-1，:)' + maxJointChange];％求解配置，并将其添加到Waypoints数组中。[qwaypoints（k，:)，solutioninfo] = gik（qwaypoints（k-1，:)，，，...heightAboveTable,...distanceFromCup, alignWithCup,...fixOrientation, limitJointChange);结尾

生成された軌跡の可視化

ウェイポイントの間を内挿して滑らかな軌跡を生成します。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 =零（Numframes，3）;为了k = 1: numFrames gripperPosition (k,:) = tform2trvec(getTransform(lbr,qInterp(k,:),...））;结尾

初期コンフィギュレーションロボット，とととにに表示ます。。

数字;显示（LBR，Qwaypoints（1，:)，，，'PreservePlot'， 错误的）;抓住on示例HelperPlotCupAndtable（Cupheight，Cupradius，Cupposition）；P = Plot3（GripperPosition（1,1），GripperPosition（1,2），GripperPosition（1,3））;

图包含一个轴对象。轴对象包含34个类型补丁的对象。These objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, cupFrame, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh.

マニピュレーターをし，の位置をし。。。

抓住on为了k = 1:size(qInterp,1) show(lbr, qInterp(k,:),'PreservePlot'， 错误的）;p.XData(k) = gripperPosition(k,1); p.YData(k) = gripperPosition(k,2); p.ZData(k) = gripperPosition(k,3); waitfor(r);结尾抓住off

生成れコンフィギュレーション后でににににににする场合は，，以下をを実行。

>>保存（'lbr_traightory.mat'，'twaypoints'，'qwaypoints'）;