【RST工具箱示例】Plan a Reaching Trajectory with Multiple Kinematic Constraints规划具有多个运动学约束的到达轨迹

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

本示例展示了如何使用广义逆运动学来规划机器人机械臂的关节空间轨迹。它结合了多个约束条件来生成引导夹持器到达桌上杯子的轨迹。这些约束确保夹持器沿直线靠近杯子，并且在靠近过程中始终保持与桌面安全距离，而无需预先确定夹持器的姿态。

设置机器人模型

本示例使用的是 KUKA LBR iiwa 7 自由度机器人机械臂模型。
importrobot 从统一机器人描述格式 (URDF) 文件生成rigidBodyTree 模型。

lbr = importrobot('iiwa14.urdf'); % 14 kg 负载版本
lbr.DataFormat = 'row';
gripper = 'iiwa_link_ee_kuka'; % 指定夹爪坐标系的名称

为杯子定义尺寸。

cupHeight = 0.2; % 定义杯子的高度
cupRadius = 0.05; % 定义杯子的半径
cupPosition = [-0.5, 0.5, cupHeight/2]; % 定义杯子的位置

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

body = rigidBody('cupFrame'); % 创建一个表示杯子中心的固定刚体
setFixedTransform(body.Joint, trvec2tform(cupPosition)) % 设置杯子的位置
addBody(lbr, body, lbr.BaseName); % 将杯子添加到机器人模型中

定义规划问题

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

• 从初始配置开始
• 机器人配置不发生突然变化
• 保持夹持器至少离“桌子”5厘米（z = 0）
• 夹持器在接近时应与杯子对齐
• 最终夹持器距离杯子中心5厘米

这个示例利用约束对象来生成满足这些条件的机器人配置。生成的轨迹包含五个配置路径点。第一个路径点 q0 设置为初始配置。使用 repmat 预先分配 qWaypoints 中的其他配置。

numWaypoints = 5; % 路径点的数量
q0 = homeConfiguration(lbr); % 设置初始配置为机器人模型的初始配置
qWaypoints = repmat(q0, numWaypoints, 1); % 预分配路径点数组

创建广义逆运动学求解器

创建一个广义逆运动学求解器，该求解器接受以下约束输入：

• 笛卡尔边界 - 限制夹持器的高度
• 位置目标 - 指定杯子相对于夹持器的位置
• 对准约束 - 使夹持器与杯子的轴线对齐
• 方向目标 - 保持夹持器在接近杯子过程中保持固定方向
• 关节位置边界 - 限制路径点之间的关节位置变化

gik = generalizedInverseKinematics('RigidBodyTree', lbr, ...
    'ConstraintInputs', {'cartesian','position','aiming','orientation','joint'});

创建约束对象

创建传递给求解器的约束对象，这些对象包含每个约束所需的参数。在每次调用求解器之间，根据需要修改这些参数。

创建一个笛卡尔边界约束，要求夹持器至少离桌面（负z方向）5厘米。所有其他值都设置为inf或-inf。

heightAboveTable = constraintCartesianBounds(gripper);
heightAboveTable.Bounds = [-inf, inf; ...
                           -inf, inf; ...
                           0.05, inf]; % 确保夹持器离桌面至少5厘米

创建一个关于杯子相对于夹持器位置的约束，允许5毫米的容差。

distanceFromCup = constraintPositionTarget('cupFrame');
distanceFromCup.ReferenceBody = gripper;
distanceFromCup.PositionTolerance = 0.005; % 设置位置容差为5毫米

创建一个对准约束，要求 iiwa_link_ee 坐标系的 z 轴大致垂直，通过将目标设置在机器人上方来实现。这一约束使夹持器与杯子的轴线对齐。

alignWithCup = constraintAiming('iiwa_link_ee');
alignWithCup.TargetPoint = [0, 0, 100]; % 设置目标点在机器人上方，保证夹持器对准杯子

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

limitJointChange = constraintJointBounds(lbr); % 限制关节位置的变化

为夹持器创建一个方向约束，容差为一度。此约束要求夹持器的方向与 TargetOrientation 属性指定的方向一致。在最终接近杯子时使用此约束来固定夹持器的方向。

fixOrientation = constraintOrientationTarget(gripper);
fixOrientation.OrientationTolerance = deg2rad(1); % 设置方向容差为一度

寻找对准杯子的配置

该配置应使夹持器距离杯子有一定距离，以便在最终接近时夹持器正确对准。

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

limitJointChange.Weights = zeros(size(limitJointChange.Weights)); % 禁用关节位置边界约束
fixOrientation.Weights = 0; % 禁用方向约束

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

distanceFromCup.TargetPosition = [0,0,intermediateDistance]; % 设置目标位置在夹持器z轴上

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

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

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

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

limitJointChange.Weights = ones(size(limitJointChange.Weights)); % 启用关节位置边界约束
fixOrientation.Weights = 1; % 启用方向约束

禁用与杯子对准的约束，因为方向约束使其多余。

alignWithCup.Weights = 0; % 禁用对准杯子的约束

将方向约束设置为保持基于先前配置的方向（qWaypoints(2,:)）。获取从夹持器到机器人基座的变换。将齐次变换转换为四元数。

fixOrientation.TargetOrientation = ...
    tform2quat(getTransform(lbr,qWaypoints(2,:),gripper)); % 设置目标方向为之前的配置

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

finalDistanceFromCup = 0.05; % 最终距离
distanceFromCupValues = linspace(intermediateDistance, finalDistanceFromCup, numWaypoints-1); % 距离逐渐减小

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

maxJointChange = deg2rad(10); % 关节位置最大变化为10度

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

for k = 3:numWaypoints
    % 更新目标位置。
    distanceFromCup.TargetPosition(3) = distanceFromCupValues(k-1);
    % 限制关节位置接近其之前的值。
    limitJointChange.Bounds = [qWaypoints(k-1,:)' - maxJointChange, ...
                               qWaypoints(k-1,:)' + maxJointChange];
    % 求解一个配置并将其添加到路径点数组中。
    [qWaypoints(k,:),solutionInfo] = gik(qWaypoints(k-1,:), ...
                                         heightAboveTable, ...
                                         distanceFromCup, alignWithCup, ...
                                         fixOrientation, limitJointChange);
end

可视化生成的轨迹

在路径点之间插值以生成平滑的轨迹。使用 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 = zeros(numFrames,3);
for k = 1:numFrames
    gripperPosition(k,:) = tform2trvec(getTransform(lbr,qInterp(k,:), ...
                                                    gripper)); % 获取夹持器的位置
end

显示机器人在初始配置下的情况，并显示桌子和杯子。

figure;
show(lbr, qWaypoints(1,:), 'PreservePlot', false); % 显示机器人初始配置
hold on
exampleHelperPlotCupAndTable(cupHeight, cupRadius, cupPosition); % 显示杯子和桌子
p = plot3(gripperPosition(1,1), gripperPosition(1,2), gripperPosition(1,3)); % 绘制夹持器位置

动画显示机械臂并绘制夹持器位置。

hold on
for k = 1:size(qInterp,1)
    show(lbr, qInterp(k,:), 'PreservePlot', false); % 显示机器人当前配置
    p.XData(k) = gripperPosition(k,1); % 更新绘图中的X数据
    p.YData(k) = gripperPosition(k,2); % 更新绘图中的Y数据
    p.ZData(k) = gripperPosition(k,3); % 更新绘图中的Z数据
    waitfor(r); % 等待下一个帧率时间
end
hold off

如果你想将生成的配置保存到MAT文件中以便以后使用，请执行以下命令：

>> save('lbr_trajectory.mat', 'tWaypoints', 'qWaypoints'); % 保存轨迹数据

功能解释：

这段代码演示了如何使用广义逆运动学规划一个机器人机械臂的关节空间轨迹，使其能够在满足多个运动学约束的条件下完成一项任务。具体来说，该示例生成一个机器人路径，使机械臂夹持器能够沿直线安全地接近一个放置在桌子上的杯子，并保持与杯子的对准。代码分多个步骤执行，包括设置机器人模型、定义规划问题、创建约束对象、使用求解器生成路径点配置以及最终可视化生成的轨迹。