规划具有多个运动学约束的到达轨迹

打开实时脚本

这个例子展示了如何使用广义逆运动学来规划机械手的关节空间轨迹。它结合了多个约束条件来生成一个轨迹，将夹持器引导到放在桌子上的杯子上。这些约束确保了夹持器在一条直线上接近杯子，并且夹持器与桌子保持安全距离，而不需要预先确定夹持器的姿势。

建立机器人模型

本例使用KUKA LBR iiwa的模型，这是一个7自由度的机械手。importrobot生成一个rigidBodyTree从存储在统一机器人描述格式(URDF)文件中的描述中获取模型。

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

定义规划问题

本例的目标是生成满足以下条件的机器人配置序列:

在主配置中启动
机器人构型没有突然变化
保持抓手距离“桌子”至少5厘米(z = 0)
当杯子靠近时，夹持器应该与杯子对齐
用手在距离杯子中心5厘米处完成

本例利用约束对象生成满足这些条件的机器人配置。生成的轨迹由五个构型路径点组成。第一个航路点，q0处，设置为主配置。预分配剩余的配置qWaypoints使用repmat．

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]

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

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

创建一个瞄准约束，要求的z轴iiwa_link_ee通过将目标放置在机器人的远上方，使框架近似垂直。的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]

为夹持器创建一个公差为一度的方向约束。控件指定的值与此约束相匹配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;

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

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

在夹具框架中为杯子设置目标位置。杯口应位于夹持器的z轴上，保持指定距离。

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

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

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

找到使夹持器沿直线移动到杯子的配置

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

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

禁用align-with-cup约束，因为定向约束使其多余。

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);结束持有从

如果需要将配置信息保存到mat文件中，请执行以下操作。

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