概述

本示例使用KINOVA Gen3操作器对检测到的对象进行排序并将其放置在长凳上。该示例使用了四个工具箱中的工具:

本示例基于两个相关示例中的关键概念:

Stateflow图表

本示例使用状态流图来调度示例中的任务。打开图表以检查内容并跟踪图表执行期间的状态转换。

编辑exampleHelperFlowChartPickPlace.sfx

图表说明了机械手如何与物体或部件相互作用。它包括基本的初始化步骤，然后是两个主要部分:

初始化机器人和环境

首先，图表创建了一个由Kinova Gen3机械手、三个要排序的部件、用于排序的货架和一个蓝色障碍组成的环境。接下来，机器人移动到主位置。

确定零件并确定放置位置

在识别阶段的第一步，必须检测部件。的exampleCommandDetectParts函数直接给出对象的姿态。用您自己的基于传感器或对象的对象检测算法替换该类。

接下来，必须对零件进行分类。的exampleCommandClassifyParts函数将部件分为两种类型，以确定放置它们的位置(顶部或底部架子)。同样，您可以用任何分类部件的方法替换此函数。

执行取放工作流程

一旦确定了部件并分配了它们的目的地，操作器必须遍历部件并将它们移动到适当的表中。

捡起物体

拾取阶段将机器人移动到物体，拾取物体，并移动到安全位置，如下图所示:

的exampleCommandComputeGraspPose函数计算抓取姿势。该类为每个部分计算一个任务空间抓取位置。接近和接近零件的中间步骤也相对于对象定义。

这个机器人用一个模拟的夹持器捡起物体。当夹持器被激活时，examplcommandActivateGripper将部件的碰撞网格添加到rigidBodyTree机器人的表示，模拟抓取它。碰撞检测在附加此对象时包含该对象。然后，机器人移动到一个远离其他部分的收缩位置。

放置对象

然后机器人把物体放在合适的架子上。

与挑选工作流程一样，放置方法和缩回位置是相对于已知的期望放置位置计算的。使用示例命令禁用夹持器ActivateGripper，将零件从机器人上移除。

移动机械手到指定的姿势

大多数任务的执行包括指令机器人在不同的指定姿势之间移动。的exampleHelperPlanExecuteTrajectoryPickPlace函数定义了一个使用非线性模型预测控制器的求解器非线性MPC(模型预测控制工具箱))，计算一个可行的、无碰撞的优化参考轨迹nlmpcmove(模型预测控制工具箱)而且checkCollision．在非线性模型预测控制算法(见[1])的定义中，障碍物被表示为球体，以确保约束雅可比矩阵的精确逼近。助手函数然后模拟机械臂在计算转矩控制下的运动，因为它跟踪参考轨迹使用jointSpaceMotionModel对象，并更新可视化。通过从状态流图调用助手函数exampleCommandMoveToTaskConfig，它定义了正确的输入。

本文对该工作流程进行了详细的研究使用KINOVA Gen3机械手规划和执行无碰撞轨迹．该控制器用于确保无碰撞运动。对于轨迹已知为无障碍的简单轨迹，可以使用轨迹生成工具执行轨迹，并使用机械手运动模型进行模拟。看到利用KINOVA Gen3机械手规划和执行任务和关节空间轨迹．

状态流图中的任务调度

这个例子使用了一个状态流图来指导MATLAB®中的工作流程。有关创建状态流程图的更多信息，请参见创建状态流图执行作为MATLAB对象(Stateflow)．

状态流图通过使用命令函数指导MATLAB中的任务执行。当命令完成执行时，它发送一个输入事件若要唤醒图表并继续执行任务执行的下一步，请参见执行独立图表(Stateflow)．

运行并可视化模拟

该模拟使用KINOVA Gen3机械手和Robotiq夹持器。中加载机器人模型.mat存档为rigidBodyTree对象。

负载(“exampleHelperKINOVAGen3GripperColl.mat”）;

初始化“拾取和放置协调器”

设置机器人初始配置。通过给出机器人模型、初始配置和末端执行器名称，创建用于处理机器人控制的协调器。

currentRobotJConfig = homeConfiguration(机器人);coordinator = exampleHelperCoordinatorPickPlace(robot,currentRobotJConfig，“爪”）;

指定用于放置不同类型对象的主配置和两种姿势。

协调员。HomeRobotTaskConfig = trvec2tform([0.4, 0, 0.6])*axang2tform([0 1 0 pi]);协调员。PlacingPose{1} = trvec2tform([0.23 0.62 0.33])*axang2tform([0 1 0 pi]);协调员。PlacingPose{2} = trvec2tform([0.23 -0.62 0.33])*axang2tform([0 1 0 pi]);

运行并可视化模拟

将协调器连接到状态流图。一旦启动，状态流图负责连续地检查检测对象的状态，将它们拾取并放置在正确的暂存区域。

协调员。流程图= exampleHelperFlowChartPickPlace(“协调员”协调员);

使用一个对话框启动拣放任务执行。点击是的在对话框中开始模拟。

答案= questdlg(“你想现在就开始收拾东西吗?”，.．.“开始工作”，“是的”，“不”，“不”）;开关回答情况下“是的”触发事件以启动“在状态流图中选择并放置”coordinator.FlowChart.startPickPlace;情况下“不”%结束选择和放置coordinator.FlowChart.endPickPlace;删除(coordinator.FlowChart);删除(协调);结束

结束Pick-and-Place任务

状态流图将在检测新对象的3次失败尝试后自动完成执行。要过早地结束选择和放置任务，取消注释并执行以下代码行或在命令窗口中按Ctrl+C。

% coordinator.FlowChart.endPickPlace;%删除(coordinator.FlowChart);%删除(协调);

观察模拟状态

在执行期间，每个时间点的活动状态在状态流图中以蓝色突出显示。这有助于跟踪机器人在什么时候做什么。您可以单击子系统以查看运行状态的详细信息。

想象拾取和放置的动作

该示例使用interactiveRigidBodyTree用于机器人可视化。可视化显示机器人在工作区域移动部件。机器人会避开环境中的障碍物(蓝色圆柱体)，并根据物体的分类将其放置在架子的顶部或底部。机器人继续工作，直到所有的部件都被安置好。

参考文献

[1]舒尔曼，J.等。"带有顺序凸优化和凸碰撞检查的运动规划"国际机器人研究杂志33.9(2014): 1251-1270。