多运动约束下的到达轨迹规划

打开生活的脚本

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

建立机器人模型

本例使用了KUKA LBR iiwa的一个7自由度机械臂模型。importrobot生成一个rigidBodyTree模型来自存储在统一机器人描述格式(URDF)文件中的描述。

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

定义杯子的尺寸。

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

在机器人模型中添加一个固定的身体，代表杯子的中心。

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

界定规划问题

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

在主配置中启动
机器人构型没有突变
保持夹持器至少高于“桌子”5厘米(z = 0)
当杯子接近时，夹持器应该与杯子对齐
在离杯子中心5厘米的地方完成夹持

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

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

创建一个generalizedInverseKinematics接受以下约束输入的求解器:

笛卡儿界限-限制夹持器的高度
位置目标-指定杯子相对于夹具的位置。
瞄准约束-使夹持器与杯轴对齐
定位目标-在接近杯子时保持钳口的固定方向
联合位置边界-限制路径点之间的联合位置的变化。

gik = generalizedInverseKinematics (“RigidBodyTree”lbr,．..“ConstraintInputs”， {笛卡儿的，“位置”，“目标”，“定位”，“联合”}）

gik = generalizedinverse运动学与属性: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: endeeffector: 'iiwa_link_ee_kuka' ReferenceBody: " TargetTransform: [4x4 double] Bounds: [3x2 double] Weights: [1 1 1]

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

distanceFromCup = constraintPositionTarget (“cupFrame”）;distanceFromCup。ReferenceBody =爪;distanceFromCup。PositionTolerance = 0.005

distanceFromCup = constraintPositionTarget with properties: EndEffector: 'cupFrame' ReferenceBody: 'iiwa_link_ee_kuka' TargetPosition: [0 00] PositionTolerance: 0.0050 Weights: 1

创建一个瞄准约束，要求z轴iiwa_link_ee通过将目标放置在机器人上方，使框架近似垂直。的iiwa_link_ee框架是定向的，这样的约束使夹持器与杯的轴对齐。

alignWithCup = constraintAiming (“iiwa_link_ee”）;alignWithCup。目标点= [0,0,100]

alignWithCup = constraints with properties: EndEffector: 'iiwa_link_ee' ReferenceBody: " TargetPoint: [0 0 100] AngularTolerance: 0 Weights: 1

创建一个关节位置边界约束。设置界限属性的约束，以限制关节位置的变化，基于前面的配置。

limitJointChange = constraintJointBounds (lbr)

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

为夹持器创建一个公差为一度的方向约束。该约束要求夹持器的方向匹配指定的值TargetOrientation财产。使用这个约束来固定在最后接近杯子的过程中夹持器的方向。

fixOrientation = constraintOrientationTarget(夹);fixOrientation。OrientationTolerance =函数(1)

fixOrientation = constraintOrientationTarget with properties: EndEffector: 'iiwa_link_ee_kuka' ReferenceBody: " TargetOrientation: [1 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解算器。您必须指定所有的输入约束。将该配置设置为第二个路径点。

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

找出将夹持器沿直线移动到杯子的结构

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

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

禁用与杯对齐约束，因为方向约束使其冗余。

alignWithCup。重量= 0;

根据前面的配置设置方向约束以保持方向(: qWaypoints (2)）.获得从抓取器到机器人模型底部的转换。将齐次变换转换为四元数。

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

为每个路径点定义杯和夹持器之间的距离

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

定义每个路点之间最大允许的关节位置变化。

maxJointChange函数= (10);

调用每个剩余路径点的求解器。

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

可视化生成的轨迹

在路径点之间插入以生成平滑的轨迹。使用pchip避免过冲，这可能会违反机器人的关节极限。

帧速率= 15;r = rateControl(帧速率);tFinal = 10;tWaypoints = [0, linspace (tFinal / 2、tFinal、大小(qWaypoints, 1) 1)];numFrames = tFinal *帧速率;qInterp = pchip (tWaypoints qWaypoints’,linspace (0 tFinal numFrames))”;

计算每个内插配置的夹持器位置。

gripperPosition = 0 (numFrames, 3);为1:numFrames gripperPosition(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));

图中包含一个轴对象。axis对象包含34个类型为patch, line的对象。这些对象代表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。

对机械手进行动画处理并绘制夹具位置图。

持有在为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);等待(r);结束持有从

如果需要将生成的配置保存到mat -文件中，请执行以下命令:

> > (lbr_trajectory攒钱。席”、“tWaypoints”、“qWaypoints”);