计划具有多个运动约束的轨迹

此示例显示如何使用广义逆运动学来规划机器人操纵器的关节空间轨迹。它结合了多个约束来生成一个轨迹，将夹具引导到休息在桌子上的杯子。这些约束确保夹持器以直线沿着直线接近杯子，并且夹持器保持距台上的安全距离，而不需要预先确定夹具的姿势。

建立机器人模型

此示例使用Kuka LBR IIWA的型号，这是一个自由度的自由度机器人机械手。ImportRobot.生成A.rigidBodyTree从存储在统一机器人描述格式（URDF）文件中的描述中的模型。

lbr = importrobot（'IIWA14.URDF'）;％14 kg有效载荷版lbr.dataformat ='排';爪=“iiwa_link_ee_kuka”;

为杯子定义尺寸。

Cupheight = 0.2;Cupradius = 0.05;圆形= [-0.5,0.5，CUPHEIGHT / 2];

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

身体=刚体（“cupFrame”）;setfixedtransform（body.joint，trvec2tform（cupposition））加法人（lbr，body，lbr.basename）;

定义规划问题

此示例的目标是生成一系列满足以下条件的机器人配置：

从家庭配置开始
机器人配置没有突然的变化
将夹持器保持在“表”上方至少5厘米（Z = 0）
夹具应在接近时与杯子对齐
完成与抓手5厘米从中心的杯

此示例利用约束对象来生成满足这些标准的机器人配置。生成的轨迹由五个配置航路点组成。第一个航点，q0处，被设置为家庭配置。预先分配其余配置Qwaypopitts.使用repmat。

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

创建一个GeneralizedInverseWinematics.接受以下约束输入的求解器：

笛卡尔界 - 限制夹具的高度
位置目标 - 指定杯子相对于夹具的位置。
瞄准约束 - 将夹具与杯轴对齐
定向目标 - 在接近杯子时保持夹具的固定方向
联合位置界限 - 限制航点之间联合职位的变化。

GIK = GeneralizedInversekkinematics（'刚性小组特雷'，lbr，......'constraintinpuls'，{'笛卡尔'那'位置'那'瞄准'那“定位”那'联合的'}）

GIK = GeneralizedInverseWkematics具有属性：NumConstraints：5约束inputs：{1x5 Cell}刚性小组编号：[1x1刚性小组特勒]溶剂竞争算法：'BfgsgradientProimple'ocverParameters：[1x1 struct]

创建约束对象

创建传递作为求解器的输入的约束对象。这些对象包含每个约束所需的参数。必要时将这些参数修改到求解器之间。

创建一个笛卡尔界约束，该约束要求夹具至少5厘米在表格（负Z方向上）上。所有其他值都是如此正或-inf.。

高度可爱的= ConstrameCarteSianBounds（夹具）;高度可爱.Bounds = [-inf，INF;......-inf，inf;......0.05，INF]

HeightaBoVetable =带有属性的ConstraintCartesianBounds：Endeffector：'IIWA_Link_ee_kuka'参考表：'TargetTransform：[4x4双]界限：[3x2双]重量：[1 1]

在杯子相对于夹具的位置上产生限制，具有5mm的公差。

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

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

创建一个瞄准约束，要求z轴IIWA_LINK_EE.通过将目标放置在机器人上方，框架近似垂直。这IIWA_LINK_EE.框架取向，使得该约束将夹具与杯子的轴对齐。

alignwithcup = constraintiming（'iiwa_link_ee'）;alpionWithCup.targetPoint = [0,0,100]

endffector: 'iiwa_link_ee' ReferenceBody: " TargetPoint: [0 0 100] AngularTolerance: 0 Weights: 1 .

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

LimitJointChange = ConstrameJointBounds（LBR）

LimitJointChange =带有属性的约束约束：边界：[7x2双]权重：[1 1 1 1 1 1]

为具有一个度的容差的夹具创造一个定向约束。该约束需要夹具的方向以匹配由此指定的值靶向财产。使用该约束在最终方法到杯子期间固定夹具的方向。

sixorientation = constrameorientationtarget（夹具）;sixorientation.orientationTolerance = DEG2RAD（1）

endffector: 'iiwa_link_ee_kuka' ReferenceBody: " TargetOrientation: [1 0 0 0] OrientationTolerance: 0.0175 Weights: 1

找到一个指向奖杯的配置

这种配置应将夹持器放置在距离杯的距离处，从而可以用正确对准的夹具进行最终的方法。

中级仪= 0.3;

约束对象有一个重量确定求解器如何处理冲突的约束的财产。将约束的权重设置为零禁用约束。对于此配置，禁用关节位置界限和方向约束。

limitjointchange.weights = zeros（size（limitjodechange.weights））;sixorientation.weights = 0;

在夹具框架中设置杯子的目标位置。杯子应位于指定距离处夹具的z轴上。

textingfromcup.targetPosition = [0,0，中介仪];

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

[qWaypoints(2，:)，solutionInfo] = gik(q0, heightAboveTable，......距离扫描，对齐，Sixorientation，......limitjointchange）;

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

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

limitjointchange.weights = ins（size（limitjodechange.weights））;sixorientation.weights = 1;

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

alpionWithCup.weights = 0;

设置方向约束以基于先前的配置保持方向（QwayPoints（2，:)）.得到从夹持器到机器人模型底座的转换。将齐次变换转换为四元数。

sixorientation.targetOleInation =.......tform2quat（getTransform（lbr，qwaypoints（2，:)，夹具））;

为每个航点定义杯子和夹具之间的距离

FinaldistanceFromcup = 0.05;距离romcupvalues = linspace（中介istance，finaldistancefromcup，numwaypoints-1）;

定义每个航点之间的关节位置的最大允许更改。

maxjointchange = deg2rad（10）;

为每个剩余航点致电求解器。

为了k = 3：numwaypoints%更新目标位置。distanceFromCup.TargetPosition (3) = distanceFromCupValues (k - 1);％限制关节位置，以靠近其先前的值。limitjointchange.bounds = [qwaypoints（k-1，:)' -  maxjointchange，......QwayPoints（k-1，:)'+ maxjointchange];％求解配置并将其添加到航点数组中。[qwaypoints（k，:)，solutioninfo] = gik（qwaypointss（k-1，:)，......heightAboveTable,......distanceFromCup alignWithCup,......Sixorientation，LimitJointchange）;结尾

可视化生成的轨迹

在航点之间插入以产生平滑的轨迹。用PCHIP.避免过冲，这可能违反机器人的联合限制。

克拉姆= 15;r = RATECONTROL（FRAMETED）;tfinal = 10;Twaypoints = [0，Linspace（TFINAL / 2，TFINAL，尺寸（QWAYPOINTS，1）-1）];numframes = tfinal * frameate;qinterp = pchip（twaypoints，qwaypoints'，linspace（0，tfinal，numframes））';

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

gripperposition = zeros（numframes，3）;为了k = 1：numframes gripperposition（k，:) = tform2trvec（getTransform（lbr，qinterp（k，:)，......夹具））;结尾

展示机器人的初始配置以及桌子和杯子

数字;显示（LBR，QwayPoints（1，:)，“PreservePlot”， 错误的）;抓住在examplehelperplotcupandtable（Cupheight，Cupradius，盆地）;p = plot3（格拉皮特（1,1），gripperposition（1,2），gripperposition（1,3））;

为机械手设置动画并绘制夹持器位置。

抓住在为了k = 1：大小（qtinterp，1）显示（lbr，qinterp（k，:)，“PreservePlot”， 错误的）;p.xdata（k）= gripperposition（k，1）;p.ydata（k）= gripperposition（k，2）;p.zdata（k）=格拉伯置（k，3）;等待（r）;结尾抓住离开

如果要将生成的配置保存到MAT文件以供以后使用，请执行以下操作：

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