主要内容

规划一种具有多重运动学约束的到达轨迹

打开生活的脚本

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

建立机器人模型

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

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

定义杯子的尺寸。

cupHeight = 0.2;cupRadius = 0.05;杯赛位置=[-0.5,0.5，杯赛高度/2];

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

身体= 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 = generalizedInverseKinematics (“RigidBodyTree”lbr,.．.“ConstraintInputs”, {笛卡儿的，“位置”，“目标”，“定位”，“联合”})
gik = generizedinversekinematics 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: EndEffector: '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属性:effeffector: 'cupFrame' ReferenceBody: 'iiwa_link_ee_kuka' TargetPosition: [0 00] PositionTolerance: 0.0050 weight: 1

创建一个瞄准约束，要求z轴的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 . TargetPoint: [0 0 100

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

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

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

fixOrientation = constraintOrientationTarget(夹);fixOrientation。OrientationTolerance =函数(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、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 = 0.05;distanceFromCupValues = linspace(intermediateDistance, finalDistanceFromCup up, numWaypoints-1);

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

maxJointChange函数= (10);

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

为k = 3: numWaypoints更新目标位置。distanceFromCup.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、大小(qWaypoints, 1) 1)];numFrames = tFinal *帧速率;qInterp = pchip (tWaypoints qWaypoints’,linspace (0 tFinal numFrames))”;

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

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

{

动画操作臂并绘制夹持器位置。

持有在为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”);

Baidu
map