感知环境下机械手运动规划的代码生成
这个例子展示了如何在可感知的环境中为规划机械手运动生成代码。可感知的环境可以具有不同数量的碰撞对象,这些碰撞对象可以是异构类型的组合(球体、圆柱体、网格和方框)。此示例使用convertToCollisionMesh
函数通过将原始类型对象转换为它们的网格等等物来均一碰撞对象的单元格数组。
设置机器人和环境
在本例中,您为一个使用manipulatorRRT
对象计划Kinova Gen 3机器人。
负载机器人模型
创建一个刚体树对象来建模机器人。对于本例,加载一个Kinova Gen3操纵器。
kinova = loadrobot (“kinovaGen3”DataFormat =“行”);
创建环境
真正的感知子系统可以输出一个结构,该结构指定了感知的几何类型、其尺寸和机器人基本框架中对象的姿态。为了模拟这一点,首先创建一个包含碰撞对象的结构。
geomStructType =结构(“类型”, exampleHelperCollisionEnum。盒子,...“X”0,...“Y”0,...“Z”0,...“顶点”coder.typeof (0 (3), (inf 3], [1 0]),...“半径”0,...“高度”0,...“姿势”、眼睛(4));geomStruct = geomStructType;geomStruct.Vertices = 0 (3);
接下来,根据感知子系统的感知,创建一个圆柱体、一个盒子和两个球体。
使用geomStruct
作为创建感知框的框架X
,Y
,Z
长度的1
,1
,0.1
,分别。
boxGeom = geomStruct;boxGeom。类型= exampleHelperCollisionEnum.Box; boxGeom.X = 1; boxGeom.Y = 1; boxGeom.Z = 0.1; boxGeom.Pose = trvec2tform([0 0 -0.051]);
创建一个感知半径的圆柱体1
和高度0.1
.
cylinderGeom = geomStruct;cylinderGeom.Type = exampleHelperCollisionEnum.Cylinder;cylinderGeom.Radius = 0.1;cylinderGeom.Height = 1.0;cylinderGeom.Pose = trvec2tform(0.5[0.3—0.50]);
创建两个可感知的球体,每个球体的半径为0.1
.
sphere1Geom = geomStruct;sphere1Geom.Type = exampleHelperCollisionEnum.Sphere;sphere1Geom.Radius = 0.1;sphere1Geom.Pose = trvec2tform(0.4[0.4—0.4]);sphere2Geom = geomStruct;sphere2Geom.Type = exampleHelperCollisionEnum.Sphere;sphere2Geom.Radius = 0.1;sphere2Geom.Pose = trvec2tform(-0.4[0.4—0.4]);
设置规划变量
的,exampleHelperVariableHeterogeneousPlanner
辅助函数接受机器人的起始和目标关节配置,以及表示环境的结构元素数组。在本例中,起始和目标关节配置是任意设置的,但是您可以根据需要设置它们。
startConfig = [-0.0035 1.4453 0.0334 0.0144 -0.0294 1.0600 -0.0077];goalConfig = [-1.1519 1.5243 0.3186 0.0009 0.2466 1.2090 -1.1952];env1 = [boxGeom cylinderGeom sphere1Geom sphere2Geom];
在可感知的环境中可视化机器人的启动配置env1
.
图(Name =“Env1”可见=“上”单位=“归一化”,OuterPosition=[0 0 1]) exampleHelperVisualizeVarSizeEnvironment(env1,kinova,startConfig);
为计划生成代码
为了生成MEX函数,exampleHelperVariableHeterogeneousPlanner_mex
,来自MATLAB函数exampleHelperVariableHeterogeneousPlanner
,执行该命令:
codegen (“exampleHelperVariableHeterogeneousPlanner”,...“参数”, {startConfig goalConfig,编码器。typeof (geomStruct[1正],[0 1]})
的variable_dims
的观点coder.typeof
(定点设计师)指定输入行向量geomStruct
的入口点规划函数可以具有无界和可变大小的二次元。
计划无碰撞路径
接下来,使用规划器入口点助手函数来输出无碰撞的几何平面。
planInEnv1 = exampleHelperVariableHeterogeneousPlanner (startConfig goalConfig env1);
也可以重用exampleHelperVariableHeterogeneousPlanner
为了一个不同的感知环境。在一个新的环境中规划,env2
,通过移除cylinderGeom
而且sphereGeom
从env1。
env2 = [boxGeom, sphere2Geom];planInEnv2 = exampleHelperVariableHeterogeneousPlanner (startConfig goalConfig env2);
MEX功能用于规划env2
,不需要重新生成。它有相同的行为exampleHelperVariableHeterogeneousPlanner
,但却缩短了计划时间。
要使用MEX函数,运行以下代码:
planInEnv1 = exampleHelperVariableHeterogeneousPlanner_mex (startConfig goalConfig env1);env2 = [boxGeom sphere2Geom];planInEnv2 = exampleHelperVariableHeterogeneousPlanner_mex (startConfig goalConfig env2);
可视化计划路径
将计划的输出可视化env1
而且env2。
图(Name =“env1中计划的路径”可见=“上”单位=“归一化”OuterPosition = [0, 0, 1, 1) exampleHelperVisualizeVarSizeEnvironment (env1, kinova planInEnv1);
图(Name ="计划的路径在env2"可见=“上”单位=“归一化”OuterPosition = [0, 0, 1, 1) exampleHelperVisualizeVarSizeEnvironment (env2, kinova planInEnv2);
支持功能
规划师入口功能
exampleHelperVariableHeterogeneousPlanner
在Kinova Gen 3机器人工作空间的感知环境中规划运动。helper函数接受感知到的环境作为geomStructs
参数,它是一个结构数组,其中每个元素包含用于表示碰撞对象的信息。
输入结构数组geomStructs
大小是可变的,由结构元素组成,每个元素都表示由类型
场的结构。的类型
字段存储枚举,exampleHelperCollisionEnum
,指定感知对象是否为盒子
,球
,油缸
或网
.有关如何构造碰撞对象结构的更多信息,请参见创建环境.
第二个manipulatorRRT
输入参数最多可以容纳100个对象。上界对于构建碰撞对象是必要的collisionBox
,collisionCylinder
,collisionSphere
,collisionMesh
在for循环中。
函数interpolatedPlan = exampleHelperVariableHeterogeneousPlanner(启动、目标geomStructs)此功能仅供内部使用,将来可能会被删除。。Kinova Gen 3机器人感知环境中的规划% INTEPROLATEDPLAN = exampleHelperVariableHeterogeneousPlanner(启动、目标GEOMSTRUCTS)输出Kinova的无碰撞几何平面INTEPROLATEDPLAN% Gen3机器人在一个由GEOMSTRUCTS定义的环境中,这是一个变量对象的信息的%大小的结构元素数组碰撞几何环境。中的每个结构元素% GEOMSTRUCTS的形式是:% geomStruct =结构(“类型”,exampleHelperCollisionEnum.Box,…% " X ", 0,…% " Y ", 0,…%“Z”,0,…%的“顶点”,0(3),…%“半径”,0,…%的“高度”,0,…%“姿势”,眼睛(4));的开始和目标联合配置% robot,分别指定为行向量。版权所有:The MathWorks, Inc.为环境创建一个占位符变量,它是一个单元格数组%的顶点大小可变的碰撞网格。大小%的环境是输入数组的碰撞几何% struct元素。coder.varsize (“顶点”[正3],[1 0]);顶点= 0 (3);env = repmat ({collisionMesh(顶点)},1,长度(geomStructs));加载规划器将被定义的刚体树。。rbt = loadrobot (“kinovaGen3”DataFormat =“行”);设置环境。碰撞物体的最大数量%环境所能容纳的是100。为我= coder.unroll (1:10 0)如果(我< =长度(geomStructs))对于每个结构元素,创建相应的碰撞% object (collisonBox, collisionCylinder, collisionSphere,或% collisionMesh)并将其转换为相应的网格%当量,从而使环境均一化。s = geomStructs(我);如果s.Type = = exampleHelperCollisionEnum。Box env{i} = convertToCollisionMesh(...collisionBox (s.X s.Y s.Z));elseifs.Type = = exampleHelperCollisionEnum。球env{i} = convertToCollisionMesh(...collisionSphere (s.Radius));elseifs.Type = = exampleHelperCollisionEnum。油缸env{i} = convertToCollisionMesh(...collisionCylinder (s.Radius s.Height));elseifs.Type = = exampleHelperCollisionEnum。网env{i} = collisionMesh(s.Vertices);结束指定元素的姿势。env{我}。构成= s.Pose;结束结束从刚体树中创建和设置计划器%的环境。env规划师= manipulatorRRT (rbt);计划。SkippedSelfCollisions =“父”;planner.MaxConnectionDistance = 0.4;planner.ValidationDistance = 0.05;对于可重复的结果,种子随机数生成器并存储%当前种子值。prevseed = rng (0);计划和插入。planOut = planner.plan(开始,目标);interpolatedPlan = planner.interpolate (planOut);将随机数生成器恢复到以前存储的种子。rng (prevseed);结束