【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});
endfor 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（快速扩展随机树）算法来规划机械臂的抓取与放置路径。它主要包括以下几个部分：
-
加载机器人模型和环境：
- 通过
exampleHelperLoadPickAndPlaceRRT函数加载机器人模型、初始配置和环境，并将这些对象可视化。 - 代码中使用
show函数可视化机器人模型的碰撞网格和环境物体。
- 通过
-
创建并配置路径规划器：
- 创建
manipulatorRRT对象并配置其参数，如MaxConnectionDistance和ValidationDistance，以控制路径生成的行为。 - 在不同的配置之间规划路径，并通过插值将路径细化为更小的步长，最终在仿真环境中展示该路径。
- 创建
-
调整规划器参数：
- 通过调整
MaxConnectionDistance、ValidationDistance和EnableConnectHeuristic参数来优化路径规划的性能，例如改变路径的长度、验证精度以及连接启发式。
- 通过调整
-
在末端执行器上附加物体并移动：
- 创建一个刚体表示罐子并将其附加到机械臂的末端执行器上。然后，通过路径规划器移动罐子并确保它不会与环境发生碰撞。
-
缩短路径：
- 使用
shorten函数对生成的路径进行优化，以减少路径长度，同时保持路径的可行性。
- 使用