基于点云处理和RRT路径规划的凉亭取放工作流

这个例子使用了:

打开实时脚本

为KINOVA®Gen3等机械手设置端到端取放工作流程。

本例中实现的拾取和放置工作流可以适应不同的场景、规划器、模拟平台和对象检测选项。这里显示的示例使用RRT来规划和模拟Gazebo中的机器人，使用机器人操作系统(ROS)。有关其他取放工作流程，请参见:

概述

本例使用KINOVA Gen3操作器将对象识别并回收到两个箱子中。这个例子使用了五个工具箱中的工具:

机器人系统工具箱™用于对机械手进行建模和仿真。
ROS工具箱™使用用于将MATLAB连接到Gazebo。
图像处理工具箱™而且计算机视觉工具箱利用点云处理和模拟深度相机在凉亭进行目标检测。

本例基于以下相关示例中的关键概念:

选择和放置使用RRT的操作器
开始凉亭和模拟乌龟机器人(ROS工具箱)(ROS工具箱)
三维点云配准与拼接(计算机视觉工具箱)(计算机视觉工具箱)

凉亭机器人仿真与控制

为KINOVA Gen3机器人启动基于ros的模拟器，并配置与机器人模拟器的MATLAB®连接。

本例使用一个虚拟机(VM)，其中包含可下载的ROS Melodic在这里．

启动Ubuntu®虚拟机桌面。
在Ubuntu桌面上，单击凉亭回收世界-深度感应图标开始为这个例子建造凉亭世界。
在Gazebo中指定ROS主机的IP地址和端口号，以便MATLAB®可以与机器人模拟器通信。本例中，Gazebo中的ROS主机使用的IP地址为172.21.72.160显示在桌面上。调整rosIP变量。
使用启动ROS 1网络rosinit．

rosIP =“172.16.34.129”；%启用ros的机器的IP地址rosshutdown;rosinit (rosIP, 11311);初始化ROS连接

初始化全局节点/matlab_global_node_63627，使用NodeURI http://172.16.34.1:35153/和MasterURI http://172.16.34.129:11311。

通过单击图标初始化凉亭世界后，虚拟机将一个KINOVA Gen3机器人手臂加载在一个桌子上，两边各有一个回收箱。为了模拟和控制Gazebo中的机械臂，虚拟机包含ros_kortexROS包，由KINOVA提供。

这些包使用ros_control控制关节到所需的关节位置。有关使用虚拟机的其他详细信息，请参见开始凉亭和模拟乌龟机器人(ROS工具箱)

拾起并定位任务

取放工作流是在MATLAB中实现的，包括基本的初始化步骤，然后是两个主要部分:

识别零件并确定放置它们的位置
执行取放工作流

有关使用statflow调度任务的实现，请参见使用MATLAB的状态流．

扫描环境，为RRT路径规划器构建规划场景

在开始拾取和放置工作之前，机器人要完成一组任务，以识别系统中的规划场景exampleCommandBuildWorld函数来检测要选取的对象exampleCommandDetectParts函数。

首先，机器人逐个移动到预定义的扫描姿势，并使用机载深度传感器捕获场景的一组点云。在每个扫描姿态中，通过使用读取相应的ROS转换来检索当前相机姿态rostf(ROS工具箱)而且getTransform(ROS工具箱)．扫描姿势如下所示:

一旦机器人访问了所有的扫描姿势，捕获的点云将从相机转换为世界帧使用pctransform(计算机视觉工具箱)并合并成单点云使用pcmerge(计算机视觉工具箱)．最后利用欧氏距离对点云进行分割pcsegdist(计算机视觉工具箱)．然后将得到的点云段编码为碰撞网格(参见collisionMesh)，在规划RRT路径时，容易识别为障碍。从点云到碰撞网格的过程如下图所示。

打开和关闭夹手

激活夹持器的命令，exampleCommandActivateGripper，发送一个动作请求来打开和关闭Gazebo中实现的夹持器。例如，要发送打开夹持器的请求，将使用以下代码。

[grpact,gripGoal] = rosactionclient(' / my_gen3 / custom_gripper_controller / gripper_cmd '）;gripperCommand = rosmessage(“control_msgs / GripperCommand”）;gripperCommand。位置= 0.0;gripGoal。Command = gripperCommand; sendGoal(gripAct,gripGoal);

将机械手移动到指定的姿态

大部分任务的执行包括指导机器人在不同的指定姿势之间移动。的exampleHelperMoveToTaskConfig函数定义RRT计划器manipulatorRRT对象，它通过避免与场景中指定的碰撞对象发生碰撞来规划从初始值到所需的联合配置的路径。结果路径首先被缩短，然后在所需的验证距离上进行插值。为了生成轨迹，trapveltraj函数用于为遵循梯形轮廓的每个插值路径点分配时间步长。最后，将路径点及其相关时间内插到所需的采样率(每0.1秒一次)。生成的轨迹确保机器人在接近或放置物体时，在运动的开始和结束时缓慢移动。

规划路径与规划场景一起在MATLAB中可视化。

该工作流程将在使用RRT规划器和MATLAB状态流的拾取和放置工作流的例子。有关RRT计划器的详细信息，请参见选择和放置使用RRT的操作器．对于已知路径为无障碍的简单轨迹，可以使用轨迹生成工具执行轨迹，并使用机械手运动模型进行模拟。看到使用KINOVA Gen3机械手计划和执行任务和关节空间轨迹．

ROS中的关节轨迹控制器

在生成机器人要遵循的关节轨迹后例子CommandMoveToTaskConfig函数以所需的采样率对轨迹进行采样，将其打包到联合轨迹ROS消息中，并向KINOVA ROS包中实现的联合轨迹控制器发送动作请求。

检测和分类场景中的物体

的函数exampleCommandDetectParts而且exampleCommandClassifyParts使用机器人提供的模拟末端执行器深度摄像头来检测可回收部件。由于一个完整的点云的场景是可用的构建环境步，迭代最近邻点(ICP)配准算法在pcregistericp(计算机视觉工具箱)确定哪些分段点云与应选择的对象的几何形状匹配。

启动“拾取-放置”工作流

这个模拟使用了一个KINOVA Gen3机械手和一个附加的夹持器。

负载(“exampleHelperKINOVAGen3GripperGazeboRRTScene.mat”）;rng (0)

初始化拾取-放置应用程序

设置机器人初始配置和末端执行器本体的名称。

initialRobotJConfig = [3.5797 -0.6562 -1.2507 -0.7008 0.7303 -2.0500 -1.9053];endEffectorFrame =“爪”；

通过给出机器人模型、初始配置和末端执行器名称来初始化协调器。

coordinator = exampleHelperCoordinatorPickPlaceROSGazeboScene(机器人，initialRobotJConfig, endEffectorFrame);

指定拾取-放置协调器属性。

协调员。HomeRobotTaskConfig = getTransform(robot, initialRobotJConfig, endEffectorFrame);协调员。PlacingPose{1} = trvec2tform([0.2 0.55 0.26])*axang2tform([0 0 1 pi/2])*axang2tform([0 1 0 pi]);协调员。PlacingPose{2} = trvec2tform([0.2 -0.55 0.26])*axang2tform([0 0 1 pi/2])*axang2tform([0 1 0 pi]);

逐步运行“选择-放置”应用程序

任务1:建造世界exampleCommandBuildWorldROSGazeboScene(协调);

移动到扫描姿势1搜索其他配置…现在计划…等待机器人达到所需配置捕获点云1获取相机姿态1移动到扫描姿态2现在计划…等待机器人达到所需配置捕获点云2获取相机姿态2移动到扫描姿态3搜索其他配置…现在计划…等待机器人达到所需配置捕获点云3获取相机姿态3移动到扫描姿态4现在计划…等待机器人达到所需配置捕获点云4获取相机姿势4移动到扫描姿势5现在计划…等待机器人达到所需配置捕获点云5获取相机姿势5

任务2:回到原位exampleCommandMoveToTaskConfigROSGazeboScene(协调员,coordinator.HomeRobotTaskConfig);

现在计划…等待机器人达到所需的配置

任务3:在场景中检测要挑选的对象exampleCommandDetectPartsROSGazeboScene(协调);

瓶检测到……能检测到……

任务4:选择下一个要挑选的部分remainingParts = exampleCommandPickingLogicROSGazeboScene(协调器);

而remainingParts = = true%任务5:[挑选]计算抓取姿势exampleCommandComputeGraspPoseROSGazeboScene(协调);任务6:[摘]摆出摘的姿势exampleCommandMoveToTaskConfigROSGazeboScene(协调员,coordinator.GraspPose);任务7:[采摘]激活夹持器exampleCommandActivateGripperROSGazeboScene(协调员,“上”）;%零件已被挑选任务8:[放置]摆成放置姿势exampleCommandMoveToTaskConfigROSGazeboScene(协调员,.．.coordinator.PlacingPose {coordinator.DetectedParts {coordinator.NextPart} .placingBelt});任务9:[放置]关闭夹持器exampleCommandActivateGripperROSGazeboScene(协调员,“关闭”）;%零件已放置选择下一个要选择的部分remainingParts = exampleCommandPickingLogicROSGazeboScene(协调器);移动到原位exampleCommandMoveToTaskConfigROSGazeboScene(协调员,coordinator.HomeRobotTaskConfig);结束

现在计划…等待机器人达到所需的配置

爪封闭……

现在计划…等待机器人达到所需的配置

钳子打开……

现在计划…等待机器人达到所需的配置

现在计划…等待机器人达到所需的配置

爪封闭……

现在计划…等待机器人达到所需的配置

钳子打开……

现在计划…等待机器人达到所需的配置

当拾取和放置应用程序完成时关闭rosrosshutdown;

使用NodeURI http://172.16.34.1:35153/和MasterURI http://172.16.34.129:11311关闭全局节点/matlab_global_node_63627。

想象一下在凉亭里的拾取和放置动作

凉亭世界展示了机器人在工作区域移动零件到回收箱。机器人继续工作，直到所有部件都放置好。