这个例子展示了如何使用广义逆运动学规划机器人机械手的关节空间轨迹。它结合了多种约束生成轨迹,引导夹一个杯子在桌子上休息。这些约束确保夹持方法杯成一条直线,从表中夹持保持在一个安全的距离,不需要钳子的姿势提前确定。
这个示例使用的模型库卡LBR iiwa, 7自由度机器人机械手。importrobot
生成一个rigidBodyTree
机器人模型描述存储在一个统一的描述格式(URDF)文件。
lbr = importrobot (“iiwa14.urdf”);% 14公斤有效载荷的版本lbr。DataFormat =“行”;爪=“iiwa_link_ee_kuka”;
定义尺寸的杯子。
cupHeight = 0.2;cupRadius = 0.05;cupPosition = (-0.5, 0.5, cupHeight / 2);
添加一个固定的身体代表杯的中心的机器人模型。
身体= rigidBody (“cupFrame”);setFixedTransform(身体。关节,trvec2tform (cupPosition)) addBody (lbr,身体,lbr.BaseName);
这个例子的目的是生成一系列的机器人配置满足以下标准:
开始在家里配置
没有突然改变机器人的配置
把钳子至少5厘米以上“表”(z = 0)
爪应该与杯的方法
完成与爪5厘米从杯子的中心
这个例子使用约束对象生成机器人配置满足这些标准。生成的轨迹包括五个配置路径点。第一个航点,q0处
,是设置为配置。预先分配的其他配置qWaypoints
使用repmat
。
numWaypoints = 5;q0 = homeConfiguration (lbr);qWaypoints = repmat (q0 numWaypoints 1);
创建一个generalizedInverseKinematics
解算器,它接受下列约束条件输入:
笛卡尔界限,限制了钳子的高度
一杯位置目标——指定的位置相对于爪。
一个目标约束——将杯的爪轴
的定位目标——保持一个固定的方向接近世界杯时夹
关节的位置锚点之间的界限——限制关节位置的变化。
gik = generalizedInverseKinematics (“RigidBodyTree”lbr,…“ConstraintInputs”,{笛卡儿的,“位置”,“目标”,“定位”,“联合”})
gik = generalizedInverseKinematics属性:NumConstraints: 5 ConstraintInputs: {1} x5细胞RigidBodyTree: [1 x1 RigidBodyTree] SolverAlgorithm:“BFGSGradientProjection”SolverParameters: [1 x1 struct]
创建约束对象作为输入传递给解算器。这些对象包含每个约束所需的参数。修改这些参数对解算器的调用之间的必要的。
创建一个笛卡尔边界约束,需要至少5厘米以上的爪表(负z方向)。其他值给出正
或负
。
heightAboveTable = constraintCartesianBounds(夹);heightAboveTable。界限= [-inf, inf;…负无穷到正无穷;…0.05,正]
heightAboveTable = constraintCartesianBounds属性:EndEffector: iiwa_link_ee_kuka ReferenceBody:”TargetTransform: [4 x4双]范围:[3 x2双]权重:(1 1 1)
创建一个限制杯的位置相对于爪,对5毫米的宽容。
distanceFromCup = constraintPositionTarget (“cupFrame”);distanceFromCup。ReferenceBody =爪;distanceFromCup。PositionTolerance = 0.005
distanceFromCup = constraintPositionTarget属性:EndEffector:“cupFrame”ReferenceBody:“iiwa_link_ee_kuka”TargetPosition: [0 0 0] PositionTolerance: 0.0050重量:1
创建一个目标需要的z轴的约束iiwa_link_ee
大约垂直框架,通过将目标远高于机器人。的iiwa_link_ee
面向框架,此约束使爪轴的杯子。
alignWithCup = constraintAiming (“iiwa_link_ee”);alignWithCup。“靶点= (0,0,100)
alignWithCup = constraintAiming属性:EndEffector: iiwa_link_ee ReferenceBody:”“靶点:[0 0 100]AngularTolerance: 0重量:1
创建一个联合位置边界约束。设置界限
产权约束的基于前面的配置限制关节位置的变化。
limitJointChange = constraintJointBounds (lbr)
limitJointChange = constraintJointBounds属性:范围:[7 x2双)重量:[1 1 1 1 1 1 1]
创建一个方位限制为抓手的宽容一个学位。这个约束要求爪的方向匹配指定的值TargetOrientation
财产。使用此约束修正的方向夹持在世界杯最后的方法。
fixOrientation = constraintOrientationTarget(夹);fixOrientation。OrientationTolerance =函数(1)
fixOrientation = constraintOrientationTarget属性:EndEffector: iiwa_link_ee_kuka ReferenceBody:”TargetOrientation: [1 0 0 0] OrientationTolerance: 0.0175重量:1
这个配置应该把钳子在距离杯,这最后的方法可以用钳子合理对齐。
intermediateDistance = 0.3;
约束对象有一个权重
属性决定了如何解决对待冲突的约束。设置约束的权重为零禁用约束。对于这个配置,禁用联合边界位置和方位限制。
limitJointChange。权重= zeros(size(limitJointChange.Weights)); fixOrientation.Weights = 0;
设置的目标位置杯夹持框架。杯子应该躺在z轴夹在指定的距离。
distanceFromCup。TargetPosition = (0, 0, intermediateDistance);
求解机器人配置满足输入限制使用gik
解算器。您必须指定的所有输入约束。设置配置第二个路标。
[qWaypoints (2:), solutionInfo] = gik (heightAboveTable q0处,…distanceFromCup、alignWithCup fixOrientation,…limitJointChange);
重新启用关节的位置和方向的限制。
limitJointChange。权重= ones(size(limitJointChange.Weights)); fixOrientation.Weights = 1;
禁用align-with-cup约束,方位限制使其冗余。
alignWithCup。重量= 0;
设置面向的方位限制基于前面的配置(:qWaypoints (2)
)。得到的变换抓手机器人的基础模型。齐次变换转换为一个四元数。
fixOrientation。TargetOrientation =…tform2quat (getTransform (lbr qWaypoints(2:),爪));
定义每个路标的杯子和夹具之间的距离
finalDistanceFromCup = 0.05;distanceFromCupValues = linspace (intermediateDistance finalDistanceFromCup numWaypoints-1);
定义的最大允许每个路标之间关节位置的变化。
maxJointChange函数= (10);
调用每个剩余的解决路径。
为k = 3: numWaypoints%更新目标位置。distanceFromCup.TargetPosition (3) = distanceFromCupValues (k - 1);%限制关节的位置躺接近其先前的值。limitJointChange。界限= [qWaypoints(k-1,:)' - maxJointChange,…:qWaypoints (k - 1) ' + maxJointChange];%求出一个配置,并将其添加到路径点的数组。[qWaypoints (k,:), solutionInfo] = gik (qWaypoints (k - 1:)…heightAboveTable,…distanceFromCup alignWithCup,…fixOrientation limitJointChange);结束
路径点之间插入生成光滑的轨迹。使用pchip
避免过激,这可能违反的共同限制机器人。
帧速率= 15;r = rateControl(帧速率);tFinal = 10;tWaypoints = [0, linspace (tFinal / 2、tFinal、大小(qWaypoints, 1) 1)];numFrames = tFinal *帧速率;qInterp = pchip (tWaypoints qWaypoints’, linspace (0 tFinal numFrames))”;
为每个插值计算出夹持位置配置。
gripperPosition = 0 (numFrames, 3);为k = 1: numFrames gripperPosition (k) = tform2trvec (getTransform (lbr, qInterp (k,:)…爪));结束
显示机器人的初始配置表和杯子
图;显示(lbr qWaypoints (1:)“PreservePlot”、假);持有在exampleHelperPlotCupAndTable (cupHeight cupRadius cupPosition);p = plot3 (gripperPosition (1, 1), gripperPosition (1、2), gripperPosition (1、3));
动画的机械手和情节夹持位置。
持有在为k = 1:尺寸(qInterp 1)显示(lbr, qInterp (k,:)“PreservePlot”、假);p.XData (k) = gripperPosition (k, 1);p.YData (k) = gripperPosition (k, 2);p.ZData (k) = gripperPosition (k, 3);等待(r);结束持有从
如果你想保存生成的配置MAT-file供以后使用,执行以下:
> > (lbr_trajectory攒钱。席”、“tWaypoints”、“qWaypoints”);