【RST示例】Pick and Place Using RRT for Manipulators使用 RRT（快速扩展随机树）进行机械臂的抓取与放置

使用 RRT（快速扩展随机树）进行机械臂的抓取与放置

使用机械臂在环境中抓取和放置物体可能需要路径规划算法，例如快速扩展随机树（RRT）规划器。该规划器在关节配置空间中探索，并寻找不同机器人配置之间的无碰撞路径。本示例展示了如何使用 manipulatorRRT 对象来调整规划器参数，并基于 Franka Emika™ Panda 机器人的 rigidBodyTree 模型，在两个关节配置之间规划路径。在调整了规划器参数后，机器人机械臂规划出一条路径，将一个罐子从一个地方移动到另一个地方。

加载机器人模型和环境

使用 exampleHelperLoadPickAndPlaceRRT 函数加载机器人及其环境。该函数输出三个变量：

• franka — 一个 Franka Emika Panda 机器人模型，作为 rigidBodyTree 对象。模型已经过修改，移除了一些总是处于碰撞状态的相邻碰撞网格，并根据可行性调整了位置限制。
• config — 机器人的初始关节位置配置。
• env — 代表机器人环境的一组碰撞物体，作为单元数组。路径规划器会检查自碰撞和与该环境的碰撞。

[franka, config, env] = exampleHelperLoadPickAndPlaceRRT;

可视化机器人模型的碰撞网格和环境物体。

figure("Name","Pick and Place Using RRT",...
    "Units","normalized",...
    "OuterPosition",[0, 0, 1, 1],...
    "Visible","on");
show(franka, config, "Visuals", "off", "Collisions", "on");
hold on
for i = 1:length(env)
    show(env{i});
end

规划器

创建 RRT 路径规划器，并指定机器人模型和环境。定义一些参数（稍后会进行调整），并指定机器人的起始和目标配置。

planner = manipulatorRRT(franka, env);
planner.SkippedSelfCollisions = 'parent';

planner.MaxConnectionDistance = 0.3;
planner.ValidationDistance = 0.1;

startConfig = config;
goalConfig = [0.2371, -0.0200, 0.0542, -2.2272, 0.0013, 2.2072, -0.9670, 0.0400, 0.0400];

在配置之间规划路径。RRT 规划器应该生成一棵快速扩展的随机配置树，以探索空间并最终返回通过环境的无碰撞路径。在规划之前，重置随机数生成器以获得可重复的结果。

rng('default');
path = plan(planner, startConfig, goalConfig);

为了可视化整个路径，将路径插值为小步长。默认情况下，interpolate 函数基于 ValidationDistance 属性生成所有经过可行性（碰撞检查）的配置。

interpStates = interpolate(planner, path);

for i = 1:2:size(interpStates, 1)
    show(franka, interpStates(i,:),...
        "PreservePlot", false,...
        "Visuals", "off",...
        "Collisions", "on");
    title("Plan 1: MaxConnectionDistance = 0.3")
    drawnow;
end

调整规划器

通过修改 MaxConnectionDistance、ValidationDistance 和 EnableConnectHeuristic 属性来调整路径规划器的参数。
将 MaxConnectionDistance 属性设置为更大的值会导致规划路径中的运动更长，但也使规划器能够更贪婪地探索空间。使用 tic 和 toc 函数对 plan 函数进行计时，以参考这些参数如何影响执行时间。

planner.MaxConnectionDistance = 5;
tic
path = plan(planner, startConfig, goalConfig);
toc

观察路径的变化。由于较大的连接距离，机械臂摆动得更高。

interpStates = interpolate(planner, path);

for i = 1:2:size(interpStates, 1)
    show(franka, interpStates(i,:),...
        "PreservePlot", false,...
        "Visuals", "off",...
        "Collisions", "on");
    title("Plan 2: MaxConnectionDistance = 5")
    drawnow;
end

将 ValidationDistance 设置为较小的值可以更精细地验证沿路径边缘的运动。增加路径上的配置验证数量会导致更长的规划时间。在存在大量碰撞物体的拥挤环境中，较小的值很有用。由于验证距离较小，interpStates 具有更多的元素。为了更快地可视化，for 循环在此步骤中跳过更多状态以加快可视化速度。

planner.MaxConnectionDistance = 0.3;
planner.ValidationDistance = 0.01;

tic
path = plan(planner, startConfig, goalConfig);
toc
interpStates = interpolate(planner, path);
for i = 1:10:size(interpStates, 1)
    show(franka, interpStates(i,:),...
        "PreservePlot", false,...
        "Visuals", "off",...
        "Collisions", "on");
    title("Plan 3: ValidationDistance = 0.01")
    drawnow;
end

连接启发式允许规划器贪婪地连接起始树和目标树。在环境不太拥挤的地方，这种启发式有助于缩短规划时间。然而，在拥挤的环境中，贪婪行为会导致无效的连接尝试。将 EnableConnectHeuristic 设置为 false 可能会延长规划时间和路径长度，但在给定迭代次数的情况下，成功找到路径的概率更高。

planner.ValidationDistance = 0.1;
planner.EnableConnectHeuristic = false;

tic
path = plan(planner, startConfig, goalConfig);
toc
interpStates = interpolate(planner, path);
for i = 1:2:size(interpStates, 1)
    show(franka, interpStates(i,:), ...
        "PreservePlot", false,...
        "Visuals", "off",...
        "Collisions", "on");
    title("Plan 4: EnableConnectHeuristic = false")
    drawnow;
end

将罐子附加到末端执行器

在为期望的行为调整规划器之后，遵循抓取和放置的工作流程，将物体通过环境移动。此示例将一个圆柱体（罐子）附加到机器人的末端执行器，并将其移动到新的位置。对于每个配置，规划器检查与圆柱体网格的碰撞。

% 创建一个刚体表示罐子
cylinder1 = env{3};
canBody = rigidBody("myCan");
canJoint = rigidBodyJoint("canJoint");

% 获取机器人手部的当前姿态。
startConfig = path(end, :);
endEffectorPose = getTransform(franka, startConfig, "panda_hand");

% 将罐子放置在末端执行器夹爪中。
setFixedTransform(canJoint, endEffectorPose\cylinder1.Pose); 

% 将碰撞几何体添加到刚体中。
addCollision(canBody, cylinder1, inv(cylinder1.Pose));
canBody.Joint = canJoint;

% 将刚体添加到机器人模型中。
addBody(franka, canBody, "panda_hand");

% 从环境中移除物体。
env(3) = [];

在罐子附加到机械臂后，为放置物体指定一个目标配置。然后，使用修改后的机器人，使用新的机器人模型创建一个规划实例。修改规划器参数。规划从起始位置到目标位置的路径。可视化路径。注意罐子是如何绕过障碍物的。

goalConfig = [-0.6564, 0.2885, -0.3187, -1.5941, 0.1103, 1.8678, -0.2344, 0.04, 0.04];
planner = manipulatorRRT(franka, env);
planner.SkippedSelfCollisions = 'parent';
planner.MaxConnectionDistance = 1;
planner.ValidationDistance = 0.2;
planner.EnableConnectHeuristic = false;
path = plan(planner, startConfig, goalConfig);

interpStates = interpolate(planner, path);

hold off
show(franka, config, "Visuals", "off", "Collisions", "on");
hold on
for i = 1:length(env)
    show(env{i});
end

for i = 1:size(interpStates, 1)
    show(franka, interpStates(i,:),...
        "PreservePlot", false,...
        "Visuals", "off",...
        "Collisions", "on");
    title("Plan 5: Place the Can")
    drawnow;
    if i == (

size(interpStates, 1))
        view([80, 7])
    end
end

缩短规划的路径

要缩短路径，请使用 shorten 函数并指定迭代次数。将 ValidationDistance 属性设置为较小的值，并结合大量迭代，可能会导致较长的计算时间。

shortenedPath = shorten(planner, path, 20);

interpStates = interpolate(planner, shortenedPath);
for i = 1:size(interpStates, 1)
    show(franka, ...
        interpStates(i, :),  ...
        "PreservePlot", false, ...
        "Visuals", "off", ...
        "Collisions", "on");
    drawnow;
    title("Plan 6: Shorten the Path")
    if i > (size(interpStates, 1) - 2)
        view([80, 7])
    end
end
hold off

功能解释与代码注释

这段代码展示了如何使用 RRT（快速扩展随机树）算法来规划机械臂的抓取与放置路径。它主要包括以下几个部分：

1. 加载机器人模型和环境：

   • 通过 exampleHelperLoadPickAndPlaceRRT 函数加载机器人模型、初始配置和环境，并将这些对象可视化。
   • 代码中使用 show 函数可视化机器人模型的碰撞网格和环境物体。

2. 创建并配置路径规划器：

   • 创建 manipulatorRRT 对象并配置其参数，如 MaxConnectionDistance 和 ValidationDistance，以控制路径生成的行为。
   • 在不同的配置之间规划路径，并通过插值将路径细化为更小的步长，最终在仿真环境中展示该路径。

3. 调整规划器参数：

   • 通过调整 MaxConnectionDistance、ValidationDistance 和 EnableConnectHeuristic 参数来优化路径规划的性能，例如改变路径的长度、验证精度以及连接启发式。

4. 在末端执行器上附加物体并移动：

   • 创建一个刚体表示罐子并将其附加到机械臂的末端执行器上。然后，通过路径规划器移动罐子并确保它不会与环境发生碰撞。

5. 缩短路径：

   • 使用 shorten 函数对生成的路径进行优化，以减少路径长度，同时保持路径的可行性。