利用URDF导入的机器人模型生成运动规划代码

打开生活的脚本

这个例子展示了如何使用从URDF文件导入的机器人模型来执行代码生成来规划运动。对于本例，您使用manipulatorRRT对象，该对象具有rigidBodyTree机器人模型在两种构型机器人之间寻找一条无障碍路径。在MATLAB®中验证算法后，使用算法中生成的MEX文件可视化机器人运动。

写算法到规划路径

创建一个函数,iiwaPathPlanner，它使用manipulatorRRT对象为KUKA LBR iiwa 14机器人模型在充满障碍的环境中规划两种配置之间的路径。

函数path = iiwaPathPlanner(startConfig, goalConfig, collisionDims, collisionposed)% # codegen机器人= iiwaForCodegen (“行”）;collisionObjects =细胞(1、长度(collisionDims));为i = 1:长度(collisionDims)开关长度(collisionDims{我})情况下1 sphereRadius = collisionDims{i};collisionObjects{我}= collisionSphere (sphereRadius {1});collisionObjects{}。构成= collisionPoses {};情况下2 cylinderDims = collisionDims{i};collisionObjects{i} = collisionCylinder(columndims {1}， columndims {2});collisionObjects{}。构成= collisionPoses {};情况下3 boxDims = collisionDims{i};collisionObjects{i} = collisionBox(boxDims{1}， boxDims{2}， boxDims{3});collisionObjects{}。构成= collisionPoses {};结束结束规划师=操纵者rt(机器人，collisionObjects);path = plan(planner, startConfig, goalConfig);结束

该算法充当标准RRT运动规划调用的包装器。它接受标准输入，并返回一组机器人构型向量作为路径。因为您不能使用句柄对象作为代码生成所支持的函数的输入或输出。在函数中加载机器人。保存iiwaPathPlanner函数在当前文件夹中。

在MATLAB中验证运动规划算法

在MATLAB中验证运动规划算法，然后生成代码。

进口库卡LBR iiwa 14机器人模型作为rigidBodyTree对象。设置数据格式为“行”．

机器人= importrobot (“iiwa14.urdf”）;机器人。DataFormat =“行”；

创建rigidBodyTree机器人模型的代码生成功能。

writeAsFunction(机器人,“iiwaForCodegen”）;

使用碰撞原语创建一个带有障碍物的简单环境。

env = {collisionBox(0.5, 0.5, 0.05)，collisionSphere(0.15)};env{1}。Pose = trvec2tform([0.35 0.35 0.3]);env{2}。Pose = trvec2tform([-0.25 0.1 0.6]);

定义一个开始和目标配置。你必须指定一个不与障碍物重叠的起点和目标。否则，计划器将抛出错误。

startConfig = robot.homeConfiguration;goalConfig = [-2.9236 1.8125 0.6484 1.6414 -0.4698 -0.4181 0.3295];

想象机器人的初始和最终位置。

图;显示(机器人,startConfig);持有所有；显示(机器人,goalConfig);显示(env {1});显示(env {2});

从环境中提取碰撞数据。

collisionDims = {{env{1}。X env{1}。Y env{1}还是z}, {env {2} .Radius}};collisionPoses = cellfun (@ (x) (x.Pose),操作环境,“UniformOutput”、假);

计划的路径。

路径= iiwaPathPlanner (startConfig goalConfig、collisionDims collisionPoses);

可视化的机器人。设置“FastUpdate”选择的显示方法真正的以获得平滑的动画。

图;ax =显示(机器人,startConfig);持有所有显示(env {1},“父”、ax);显示(env {2},“父”、ax);rrt = manipulatorRRT(机器人,env);interpPath =插入(rrt路径);为i = 1:size(interpPath,1) show(robot,interpPath(i，:)，“PreservePlot”假的,“FastUpdate”,真正的);drawnow;结束

运动规划算法代码生成

你可以使用codegen(MATLAB编码器)函数或MATLAB编码器(MATLAB编码器)应用程序生成代码。对于本例，通过调用生成MEX文件codegen在MATLAB命令行。方法为函数的每个输入指定样例输入参数arg游戏输入参数。

调用codegen函数并指定单元格数组中的输入参数。此函数创建一个单独的iiwaPathPlanner_mex函数使用。还可以通过使用options输入参数生成C代码。这一步可能需要一些时间。

codegeniiwaPathPlannerarg游戏{startConfig, goalConfig、collisionDims collisionPoses}

代码生成成功。

使用生成的MEX函数验证结果

通过调用指定的开始和目标配置的运动规划算法的MEX版本，以及来自环境的碰撞数据来规划路径。

路径= iiwaPathPlanner_mex (startConfig goalConfig、collisionDims collisionPoses);

用运动规划算法的MEX版本计算出的机器人配置来可视化机器人。设置“FastUpdate”选择的显示方法真正的以获得平滑的动画。

图;ax = show(robot, startConfig);持有所有显示(env {1},“父”、ax);显示(env {2},“父”、ax);interpPath =插入(rrt路径);为i = 1:size(interpPath,1) show(robot,interpPath(i，:)，“PreservePlot”假的,“FastUpdate”,真正的);drawnow;结束